// Copyright 2024 XMOS LIMITED.
// This Software is subject to the terms of the XMOS Public Licence: Version 1.
#include <string.h>
#include <stdlib.h>
#include <xcore/assert.h>
#include <debug_print.h>
#include "cmds.h" // Autogenerated
#include "cmd_offsets.h" // Autogenerated
#include "stages/reverb_plate.h"
#include "dsp/_helpers/generic_utils.h"


static inline int32_t _calc_decay_diffusion_2(int32_t decay)
{
    // decay_diffusion_2 = clip(decay+0.15, 0.25, 0.5), 
    // if decay > 0.35, clip to 0.5 before adding 0.15 to avoid overflow if decay > 0.85
    // otherwise, add 0.15 and clip the lower limit
    int32_t decay_diffusion_2 = (decay > 751619276) ? 1073741824 : decay + 322122547;
    decay_diffusion_2 = decay_diffusion_2 < 536870912 ? 536870912 : decay_diffusion_2;
    // printf("decay: %ld, dd2: %ld\n", decay, decay_diffusion_2);
    return decay_diffusion_2;
}

static inline int32_t _calc_input_diffusion_2(int32_t early_diffusion)
{
    // input_diffusion_2 = 5/6*early_diffusion 
    // 1789558784 = 5/6 quantized to 16b
    int32_t input_diffusion_2 = apply_gain_q31(early_diffusion, 1789558784);
    // printf("early: %ld, id2: %ld\n", early_diffusion, input_diffusion_2);
    return input_diffusion_2;
}

void reverb_plate_init(module_instance_t* instance,
                 adsp_bump_allocator_t* allocator,
                 uint8_t id,
                 int n_inputs,
                 int n_outputs,
                 int frame_size)
{
    xassert(n_inputs == 2 && "Reverb should have 2 inputs and outputs");
    xassert(n_inputs == n_outputs && "Reverb should have the same number of inputs and outputs");
    reverb_plate_state_t *state = instance->state;
    reverb_plate_config_t *config = instance->control.config;
    reverb_plate_constants_t *constants = instance->constants;

    memset(state, 0, sizeof(reverb_plate_state_t));

    state->n_inputs = n_inputs;
    state->n_outputs = n_outputs;
    state->frame_size = frame_size;

    float fs = constants->sampling_freq;
    uint32_t max_predelay = constants->max_predelay;
    uint32_t predelay = config->predelay;
    int32_t decay_diff2 = _calc_decay_diffusion_2(config->decay);
    int32_t decay_diff1 = config->late_diffusion;
    int32_t in_diff1 = config->early_diffusion;
    int32_t in_diff2 = _calc_input_diffusion_2(config->early_diffusion);

    xassert(n_inputs == 2); // Stereo only implementation

    state->rv.lowpasses[0] = lowpass_1ord_init(config->bandwidth);
    state->rv.lowpasses[1] = lowpass_1ord_init(config->damping);
    state->rv.lowpasses[2] = lowpass_1ord_init(config->damping);

    uint8_t *reverb_heap = adsp_bump_allocator_malloc(allocator, REVERB_PLATE_STAGE_REQUIRED_MEMORY(fs, max_predelay));
    memset(reverb_heap, 0, REVERB_PLATE_STAGE_REQUIRED_MEMORY(fs, max_predelay));

    state->rv.decay = config->decay;
    state->rv.pre_gain = config->pregain;
    state->rv.wet_gain1 = config->wet_gain1;
    state->rv.wet_gain2 = config->wet_gain2;
    state->rv.dry_gain = config->dry_gain;

    adsp_reverb_plate_init_filters(&state->rv, fs, decay_diff1, decay_diff2, in_diff1, in_diff2, max_predelay, predelay, reverb_heap);
}

void reverb_plate_process(int32_t **input, int32_t **output, void *app_data_state)
{
    reverb_plate_state_t *state = app_data_state;
    int32_t *inl = input[0];
    int32_t *inr = input[1];
    int32_t *outl = output[0];
    int32_t *outr = output[1];
    int32_t out_lr[2];
    int j = 0;
    do
    {
        adsp_reverb_plate(&state->rv, out_lr, (*inl++), (*inr++));
        *outl++ = out_lr[0];
        *outr++ = out_lr[1];

    } while (++j < state->frame_size);
}

void reverb_plate_control(void *module_state, module_control_t *control)
{
    xassert(module_state != NULL);
    reverb_plate_state_t *state = module_state;
    xassert(control != NULL);
    reverb_plate_config_t *config = control->config;

    if(control->config_rw_state == config_write_pending)
    {
        state->rv.pre_gain = config->pregain;
        state->rv.wet_gain1 = config->wet_gain1;
        state->rv.wet_gain2 = config->wet_gain2;
        state->rv.dry_gain = config->dry_gain;
        state->rv.decay = config->decay;
        state->rv.predelay.delay = config->predelay;
        // damping is always at least 1
        int32_t damp2 = (uint32_t)(1<<31) - config->bandwidth;
        state->rv.lowpasses[0].damp_1 = config->bandwidth;
        state->rv.lowpasses[0].damp_2 = damp2;
        damp2 = (uint32_t)(1<<31) - config->damping;

        for (unsigned i = 0; i < 2; i ++) {
            state->rv.mod_allpasses[i].feedback = config->late_diffusion;
            state->rv.allpasses[i].feedback = config->early_diffusion;
            state->rv.lowpasses[i + 1].damp_1 = config->damping;
            state->rv.lowpasses[i + 1].damp_2 = damp2;
            state->rv.allpasses[i + 2].feedback = _calc_input_diffusion_2(config->early_diffusion);
            state->rv.allpasses[i + 4].feedback = _calc_decay_diffusion_2(config->decay);
        }
        control->config_rw_state = config_none_pending;
    }
    else if(control->config_rw_state == config_read_pending)
    {
        // none of these should be changed during the reverb execution,
        // so don't really need to update the config,
        // leaving it here in case something goes really wrong,
        // so there's a way to debug
        config->pregain = state->rv.pre_gain;
        config->wet_gain1 = state->rv.wet_gain1;
        config->wet_gain2 = state->rv.wet_gain2;
        config->dry_gain = state->rv.dry_gain;
        config->decay = state->rv.decay;
        config->bandwidth = state->rv.lowpasses[0].damp_1;
        config->damping = state->rv.lowpasses[1].damp_1;
        config->predelay = state->rv.predelay.delay;
        config->late_diffusion = state->rv.mod_allpasses[0].feedback;
        config->early_diffusion = state->rv.allpasses[0].feedback;
        control->config_rw_state = config_read_updated;
    }
    else {
        // nothing to do
    }

}
