/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id:$
 *
 * Copyright (C) 2006-2007 Thom Johansen
 * Copyright (C) 2006-2007 Michael Sevakis 
 *
 * All files in this archive are subject to the GNU General Public License.
 * See the file COPYING in the source tree root for full license agreement.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/
#include "plugin.h"

PLUGIN_HEADER
PLUGIN_IRAM_DECLARE

static struct plugin_api* rb;

static ssize_t queue_size;
/* Important to leave enough room behind effect_index to allow for delays like screen updates */
static volatile int wr_index = 4;
static volatile int rd_index = 0;
static int effect_index      = 5; /* one ahead to account for preincrement of wr_index */

static unsigned long effect_thread_stack[DEFAULT_STACK_SIZE] IBSS_ATTR;
static bool stop_effect = false;

#define CHUNK_SAMPLES   128
#define CHUNK_SIZE      (CHUNK_SAMPLES*4)
#define NUM_CHUNKS      8 /* power of 2 */
#define CHUNK_MASK      (NUM_CHUNKS-1)
#define NEXT_CHUNK(x)   (((x) + 1) & CHUNK_MASK)

typedef int32_t (* buffer_t)[CHUNK_SAMPLES];
buffer_t queue;

/* Effect data */
struct lfo {
    int32_t phase, delta;
};

struct flanger {
    size_t writepos;
    size_t mask;
    int16_t *delayline;
    int32_t delay;
    int32_t dry, wet;
    int32_t send, fb;
    struct lfo lfo;
    int32_t lfoamt;
};

int16_t delayline_l[16384], delayline_r[16384];
struct flanger flanger_l, flanger_r;

static inline int32_t saw_lfo(struct lfo *l)
{
    int32_t ret = l->phase;
    l->phase += l->delta;
    return ret;
}

static inline int32_t tri_lfo(struct lfo *l)
{
    int32_t ret = (abs(l->phase + 0x3fffffff) - 0x3fffffff) << 1;
    l->phase += l->delta;
    return ret;
}

static inline int32_t par_lfo(struct lfo *l)
{
    int32_t shift_phase = l->phase >> 16;
    int32_t ret = 8*shift_phase*((1 << 15) - abs(shift_phase));
    l->phase += l->delta;
    return ret;
}

static void flanger_init(struct flanger *f, int16_t *dl, int num)
{
    rb->memset(f, 0, sizeof(*f));
    rb->memset(dl, 0, num*sizeof(*dl));
    f->mask = num - 1;
    f->delayline = dl;
}

static void ICODE_ATTR flanger_process(int16_t *data, struct flanger *d, int num)
{
    int i;
    size_t mask = d->mask;
    size_t writepos = d->writepos;
    int delay;

    for (i = 0; i < num; ++i) {
        delay = d->delay + FRACMUL(par_lfo(&d->lfo), d->lfoamt);
        size_t r = writepos - (delay >> 15);
        int32_t frac = 0x8000 - (delay & 0x7fff);
        
        const int32_t xm1 = d->delayline[(r - 1) & mask];
        const int32_t x0 = d->delayline[r & mask];
        const int32_t xp1 = d->delayline[(r + 1) & mask];
        const int32_t xp2 = d->delayline[(r + 2) & mask];
        
        const int32_t c = (xp1 - xm1)/2;
        const int32_t v = x0 - xp1;
        const int32_t w = c + v;
        const int32_t a = w + v + (xp2 - x0)/2;
        const int32_t b = w + a;
        
        int32_t output = (((((a*frac >> 15) - b)*frac >> 15) + c)*frac >> 15) + x0;
        int32_t input = *data;
        d->delayline[writepos] = (input*d->send >> 15) + (output*d->fb >> 15);
        *data = (input*d->dry >> 15) + (output*d->wet >> 15);
        data += 2;
        writepos = (writepos + 1) & mask;
    }
    d->writepos = writepos;
}

static void ICODE_ATTR play_callback(unsigned char **start, size_t *size)
{
    rd_index = NEXT_CHUNK(rd_index);
    *start = (unsigned char *)queue[rd_index];
    *size  = CHUNK_SIZE;
}

static int ICODE_ATTR record_callback(int status)
{
    (void)status;
    wr_index = NEXT_CHUNK(wr_index);
    rb->pcm_record_more(queue[wr_index], CHUNK_SIZE);
    return 0;
}

static void ICODE_ATTR effect_thread(void)
{
    int y = 0;
    while (!stop_effect)
    {
        /* Wait for at least one chunk of data available */
        while (wr_index == effect_index)
            rb->yield();

        do
        {
            int16_t *ptr = (int16_t *)queue[effect_index];

            rb->reset_poweroff_timer();
            flanger_process(ptr, &flanger_l, CHUNK_SAMPLES);
            flanger_process(ptr + 1, &flanger_r, CHUNK_SAMPLES);

            effect_index = NEXT_CHUNK(effect_index);

            if (++y >= 8)
            {
                rb->yield();
                y = 0;
            }
        }
        while (effect_index != wr_index);
    }
}

void hz_format(char* buf, size_t bufsize, int val, const char* unit)
{
    rb->snprintf(buf, bufsize, "%d.%d %s", val / 10, val % 10, unit);
}

void delay_format(char* buf, size_t bufsize, int val, const char* unit)
{
    int v = (1 + val*val)/4;
    rb->snprintf(buf, bufsize, "%d.%d %s", v / 10, v % 10, unit);
}

int dry_level = 50, wet_level = 50, delay_send = 100;
int delay_time1 = 10, delay_time2 = 30;
int feedback_level = 50, lfo_freq = 3, lfo_phasediff = 0;

void set_flanger_params(int val)
{
    (void)val;
    int32_t d1 = (1 + delay_time1*delay_time1)*441/400 << 15;
    int32_t d2 = (1 + delay_time2*delay_time2)*441/400 << 15;
    flanger_l.dry = flanger_r.dry = dry_level*0x7fff/100;
    flanger_l.wet = flanger_r.wet = wet_level*0x7fff/100;
    flanger_l.send = flanger_r.send = delay_send*0x7fff/100;
    flanger_l.delay = flanger_r.delay = (d2 + d1)/2;
    flanger_l.fb = flanger_r.fb = feedback_level*0x7fff/100;
    flanger_l.lfo.delta = flanger_r.lfo.delta = 0xffffffff/441000*lfo_freq;
    flanger_l.lfoamt = flanger_r.lfoamt = d2 - flanger_l.delay;
    flanger_r.lfo.phase = flanger_l.lfo.phase + 0x28f5c28*lfo_phasediff;
}

void flanger_menu(void)
{
    int result, select = 0;
    MENUITEM_STRINGLIST(
        menu, "Flanger", NULL,
        "Dry mix",
        "Wet mix",
        "Send level",
        "Delay 1",
        "Delay 2",
        "Feedback",
        "LFO frequency",
        "LFO phase difference"
    );

    while ((result = rb->do_menu(&menu, &select)) != GO_TO_PREVIOUS) {
        switch (select) {
        case 0:
            rb->set_int("Dry mix", "%", UNIT_INT, &dry_level, set_flanger_params, 1, 0, 100, NULL);
            break;
        case 1:
            rb->set_int("Wet mix", "%", UNIT_INT, &wet_level, set_flanger_params, 1, 0, 100, NULL);
            break;
        case 2:
            rb->set_int("Send level", "%", UNIT_INT, &delay_send, set_flanger_params, 1, 0, 100, NULL);
            break;
        case 3:
            rb->set_int("Delay 1", "ms", UNIT_INT, &delay_time1, set_flanger_params, 1, 3, 100, delay_format);
            break;
        case 4:
            rb->set_int("Delay 2", "ms", UNIT_INT, &delay_time2, set_flanger_params, 1, 3, 100, delay_format);
            break;
        case 5:
            rb->set_int("Feedback", "%", UNIT_INT, &feedback_level, set_flanger_params, 2, -100, 100, NULL);
            break;
        case 6:
            rb->set_int("LFO frequency", "Hz", UNIT_INT, &lfo_freq, set_flanger_params, 2, 0, 200, hz_format);
            break;
        case 7:
            rb->set_int("LFO phase difference", "%", UNIT_INT, &lfo_phasediff, set_flanger_params, 2, -100, 100, NULL);
            break;
        }

    }
}

enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
{
    int result, select = 0;
    int ingain = 45, outgain = 0;
    MENUITEM_STRINGLIST(
        menu, "Effect box", NULL,
        "Quit",
        "Source",
        "Input gain",
        "Output gain",
        "Flanger"
    );
    struct thread_entry *effect_thread_p;

    PLUGIN_IRAM_INIT(api);

    rb = api;

    queue = (buffer_t)rb->plugin_get_audio_buffer(&queue_size);
    queue = (typeof(queue))ALIGN_UP((intptr_t)queue, 4);
    rb->memset(queue, 0, CHUNK_SIZE*NUM_CHUNKS);
    rb->memset(delayline_l, 0, sizeof(delayline_l));
    rb->memset(delayline_r, 0, sizeof(delayline_r));

    rb->pcm_init_recording();
    rb->audio_set_output_source(AUDIO_SRC_PLAYBACK);
    rb->audio_set_input_source(AUDIO_SRC_LINEIN, SRCF_RECORDING);
    rb->pcm_set_frequency(SAMPR_44);
    rb->pcm_apply_settings();

    flanger_init(&flanger_l, delayline_l, sizeof(delayline_l)/sizeof(int16_t));
    flanger_init(&flanger_r, delayline_r, sizeof(delayline_r)/sizeof(int16_t));
    set_flanger_params(0);

    rb->cpu_boost(true);

    rb->pcm_play_data(play_callback, NULL, 0);
    rb->pcm_record_data(record_callback, queue[wr_index], CHUNK_SIZE);

    /* Start a thread for processing the audio */
    effect_thread_p = rb->create_thread(effect_thread, effect_thread_stack,
                                        sizeof(effect_thread_stack), 0,
                                        "effect thread", PRIORITY_RECORDING);

    rb->sound_set(SOUND_VOLUME, outgain);
    rb->sound_set(SOUND_MIC_GAIN, 100);
    rb->audio_set_recording_gain(ingain, ingain, AUDIO_GAIN_LINEIN);
    while ((result = rb->do_menu(&menu, &select)) > 0) {
        if (select == 1) {
            bool source;
            unsigned flags;

            if (rb->set_bool_options("Source", &source, "Int. mic", 0,
                                     "Line in/ext. mic", 0, NULL)) {
                if (source)
                    flags = AUDIO_SRC_MIC;
                else 
                    flags = AUDIO_SRC_LINEIN;
                rb->audio_set_input_source(flags, SRCF_RECORDING);
            }
        } else if (select == 2) {
            int min = rb->sound_min(SOUND_LEFT_GAIN);
            int max = rb->sound_max(SOUND_LEFT_GAIN);

            rb->set_int("Input gain", "dB", UNIT_DB, &ingain, NULL, 1,
                        min, max, NULL);
            rb->audio_set_recording_gain(ingain, ingain, AUDIO_GAIN_LINEIN);
        } else if (select == 3) {
            int min = rb->sound_min(SOUND_VOLUME);
            int max = rb->sound_max(SOUND_VOLUME);

            rb->set_int("Output gain", "dB", UNIT_DB, &outgain, NULL, 1,
                        min, max, NULL);
            rb->sound_set(SOUND_VOLUME, outgain);
        } else if (result == 4) {
            flanger_menu();
        }
    }

    /* stop effect thread - it will remove itself */
    stop_effect = true;
    rb->thread_wait(effect_thread_p);

    rb->pcm_play_stop();
    rb->pcm_stop_recording();
    rb->pcm_close_recording();

    rb->cpu_boost(false);

    rb->audio_set_output_source(AUDIO_SRC_PLAYBACK);
    rb->audio_set_input_source(AUDIO_SRC_PLAYBACK, SRCF_PLAYBACK);
    rb->pcm_set_frequency(HW_SAMPR_DEFAULT);
    rb->pcm_apply_settings();

    return PLUGIN_OK;
    (void)parameter;
}