#include <xcore/assert.h>
#include <xcore/thread.h>
#include <xcore/hwtimer.h>
#include <xua.h>
#include <xua_audiohub.h>
#include <stdint.h>
#include <stdbool.h>

#include <dsp/adsp.h>
#include <stages/adsp_pipeline.h>
#include <stages/adsp_control.h>
#include <adsp_generated_auto.h>
#include <adsp_instance_id_auto.h>
#include "cmds.h"

#include "app_dsp.h"

// This assumes that the application has not enabled MIDI, SPDIF, ADAT, or PDM mics.
#define N_CHANS_FROM_AUDIO (I2S_CHANS_ADC)
#define N_CHANS_TO_AUDIO (I2S_CHANS_DAC)
#define N_CHANS_TO_DSP (NUM_USB_CHAN_OUT + I2S_CHANS_ADC)
#define N_CHANS_FROM_DSP (NUM_USB_CHAN_IN + I2S_CHANS_DAC)

adsp_pipeline_t *m_dsp;

DECLARE_JOB(dsp_control, (chanend_t));

/// Fills an array of points to point to the corresponding element
/// of an input array.
static void arr_to_pointer(int32_t** pointer, unsigned* arr, int n) {
    for(int i = 0; i < n; ++i) {
        pointer[i] = &((int32_t*)arr)[i];
    }
}

/// Standard lib_xua callback function.
void UserBufferManagement(unsigned* sampsFromUsbToAudio, unsigned* sampsFromAudioToUsb) {
    xassert(NULL != m_dsp);

    // construct DSP input arrays and hand over to the DSP thread.
    int32_t* dsp_input[N_CHANS_TO_DSP];
    arr_to_pointer(&dsp_input[0], sampsFromUsbToAudio, NUM_USB_CHAN_OUT);
    arr_to_pointer(&dsp_input[NUM_USB_CHAN_OUT], sampsFromAudioToUsb, N_CHANS_FROM_AUDIO);
    adsp_pipeline_source(m_dsp, dsp_input);

    // construct the DSP output arrays and wait for output from the DSP
    // thread.
    int32_t* dsp_output[N_CHANS_FROM_DSP];
    arr_to_pointer(&dsp_output[0], sampsFromUsbToAudio, N_CHANS_TO_AUDIO);
    arr_to_pointer(&dsp_output[N_CHANS_TO_AUDIO], sampsFromAudioToUsb, NUM_USB_CHAN_IN);
    adsp_pipeline_sink(m_dsp, dsp_output);
}

// start dsp_thread
void dsp_thread(chanend_t c_gpio) {
    // Initialise the DSP instance and launch the generated DSP main function
    // as well as the control thread
    m_dsp = adsp_auto_pipeline_init();
    PAR_JOBS(
        PJOB(adsp_auto_pipeline_main, (m_dsp)),
        PJOB(dsp_control, (c_gpio))
    );
}
// end dsp_thread

// Most of the defines below rely on this q formats
#if ((Q_GAIN != 27) || (Q_SIG != 27))
#error dsp_control API will only work if the Q_GAIN and Q_SIG are being equal to 27
#endif

// Bass boost will be turned off/on by changing the peak limiter threshold
#define BASS_BOOST_OFF  1342 // -100 dB in linear q27
#define BASS_BOOST_ON   42443372 // -10 dB in linear q27

// In order to not deal with dB conversions for the volume change
// We will multiply the current target gain by a value that corresponds
// to a change of +- 3dB: + 3 dB is 1.412538, -3 dB is 0.707946
#define VOLUME_UP_INC   189587641 // 1.412538 in q27
#define VOULME_DOWN_INC 95018903 // 0.707946 in q27
#define UPPER_CAP       (1 << Q_GAIN) // 0dB in q27
#define LOWER_CAP       1342177 // -40 dB in q27

// VU meter refresh rate in cycles
#define REFRESH_TICKS   (50 * XS1_TIMER_KHZ) // 20 Hz
#define LED0_TH         13421 // -40 dB pow
#define LED1_TH         134217 // -30 dB pow
#define LED2_TH         1342177 // -20 dB pow
#define LED3_TH         13421772 // -10 dB pow

// start bass boost
static void do_bass_boost(adsp_controller_t* controller, 
                          bool* bass_boost_status) {
    *bass_boost_status = !*bass_boost_status; 
    int32_t val = *bass_boost_status? BASS_BOOST_ON : BASS_BOOST_OFF;
    adsp_stage_control_cmd_t cmd = {
        .payload_len = sizeof(int32_t),
        .payload = &val,
        .instance_id = bass_sw_stage_index,
        .cmd_id = CMD_LIMITER_PEAK_THRESHOLD
    };

    // do write until success
    while(ADSP_CONTROL_SUCCESS != adsp_write_module_config(controller, &cmd));
}
// end bass boost

// start volume
static void do_volume_control(adsp_controller_t* controller, 
                              bool volume_up) {
    // Get the current volume
    int32_t val;
    adsp_stage_control_cmd_t cmd = {
        .payload_len = sizeof(int32_t),
        .payload = &val,
        .instance_id = vol_ctl_stage_index,
        .cmd_id = CMD_VOLUME_CONTROL_TARGET_GAIN
    };

    // do read until success
    while(ADSP_CONTROL_SUCCESS != adsp_read_module_config(controller, &cmd));

    // Update the volume
    int32_t mul = (volume_up) ? VOLUME_UP_INC : VOULME_DOWN_INC;
    val = adsp_fixed_gain(val, mul); // apply gain
    val = (val > UPPER_CAP) ? UPPER_CAP : val;
    val = (val < LOWER_CAP) ? LOWER_CAP : val;
    
    // do write until success, cmd can be reused
    while(ADSP_CONTROL_SUCCESS != adsp_write_module_config(controller, &cmd));
}
// end volume

// start vu
uint8_t do_get_vu(adsp_controller_t* controller) {
    int32_t val;
    adsp_stage_control_cmd_t cmd = {
        .payload_len = sizeof(int32_t),
        .payload = &val,
        .instance_id = lim_stage_index,
        .cmd_id = CMD_LIMITER_RMS_ENVELOPE
    };

    // do read until success
    while(ADSP_CONTROL_SUCCESS != adsp_read_module_config(controller, &cmd));
    
    uint8_t led_val = 0;
    led_val = (val > LED0_TH) ? led_val + 1 : led_val;
    led_val = (val > LED1_TH) ? led_val + 2 : led_val;
    led_val = (val > LED2_TH) ? led_val + 4 : led_val;
    led_val = (val > LED3_TH) ? led_val + 8 : led_val;
    return led_val;
}
// end vu

// start dsp_control
void dsp_control(chanend_t c_gpio) {
    xassert(NULL != m_dsp);

    adsp_controller_t controller;
    adsp_controller_init(&controller, m_dsp);
    
    bool bass_boost_status = false;

    SELECT_RES(
        CASE_THEN(c_gpio, on_c_gpio))
    {
        on_c_gpio: {
            uint8_t ctrl = chan_in_byte(c_gpio);

            switch(ctrl) {
                case BASS_BOOST_SW: {
                    do_bass_boost(&controller, &bass_boost_status);
                } break;
                case VOLUME_UP: {
                    do_volume_control(&controller, true);
                } break;
                case VOLUME_DOWN: {
                    do_volume_control(&controller, false);
                } break;
                case GET_VU_LEVELS: {
                    uint8_t led_val = do_get_vu(&controller);
                    chan_out_byte(c_gpio, led_val);
                } break;
                default: {
                    xassert(false);
                } break;
            }
            continue;
        }
    }
}
// end dsp_control
