/*
 * DSP_FACTORY.c
 *
 *  Created on: May 7, 2021
 *      Author: Christopher
 */

#define ARM_MATH_CM7
#include "main.h"
#include "arm_math.h"
#include "FILTER.h"
#include "CONSTANTS.h"
#include "OPCODE.h"
#include "I2S_FACTORY.h"

//static double inputCompensator = 1.0;
//static uint8_t volumeCurrent = 50;

static arm_biquad_cascade_df2T_instance_f64 audioStream_L;
static arm_biquad_cascade_df2T_instance_f64 audioStream_R;

void INIT_FILTER(uint8_t sourceMode) {

    switch(sourceMode) {

    case INPUT_INLINE:

        arm_biquad_cascade_df2T_init_f64(&audioStream_L, 18, initFilter_192kHz_inline, pState_L_192kHz_inline);
        arm_biquad_cascade_df2T_init_f64(&audioStream_R, 18, initFilter_192kHz_inline, pState_R_192kHz_inline);
        break;

    case INPUT_BLUETOOTH:

        arm_biquad_cascade_df2T_init_f64(&audioStream_L, 8, initFilter_44_1kHz_bluetooth, pState_L_44_1kHz_bluetooth);
        arm_biquad_cascade_df2T_init_f64(&audioStream_R, 8, initFilter_44_1kHz_bluetooth, pState_R_44_1kHz_bluetooth);
        break;
    }
}

void I2S_HALFCOMPLETE_CALLBACK() {

    int * I2S1_RxBUFF = getI2S1_RxBUFF();
    int * I2S1_TxBUFF = getI2S1_TxBUFF();

    //double INSAMPLE_I2S_L[1024];
    //double INSAMPLE_I2S_R[1024];

    //double OUTSAMPLE_I2S_L[1024];
    //double OUTSAMPLE_I2S_R[1024];

    /*for (int i = 0; i < 2048; i ++){

        if ( i % 2 == 0){ // R Samples

              INSAMPLE_I2S_L[i-(i >> 1)] = (double) (I2S1_RxBUFF[i] << ADC_COMPENSATOR) * (0.01 * (double) volumeCurrent) * inputCompensator;

        } else if ( i % 2 == 1){ // R Samples

              INSAMPLE_I2S_R[i-1-((i-1)>>1)] = (double) (I2S1_RxBUFF[i] << ADC_COMPENSATOR) * (0.01 * (double) volumeCurrent) * inputCompensator;

        }
}
                 arm_biquad_cascade_df2T_f64(&audioStream_L, INSAMPLE_I2S_L, OUTSAMPLE_I2S_L, 1024);
                 arm_biquad_cascade_df2T_f64(&audioStream_R, INSAMPLE_I2S_R, OUTSAMPLE_I2S_R, 1024);*/

                for (int i = 0; i < 2; i ++){

                    I2S1_TxBUFF[i] = I2S1_RxBUFF[i];
                 /*  if (i % 2 == 0) { // L Samples

                       I2S1_TxBUFF[i] = (int)OUTSAMPLE_I2S_L[i-(i >> 1)];

                 } else if ( i % 2 == 1) { // R Samples

                       I2S1_TxBUFF[i] = (int)OUTSAMPLE_I2S_R[i-1-((i-1)>>1)];

                 }*/
            }
               DMA1_Stream1->CR |= DMA_SxCR_EN;
}

void I2S_TRANSFERCOMPLETE_CALLBACK() {

    int * I2S1_RxBUFF = getI2S1_RxBUFF();
    int * I2S1_TxBUFF = getI2S1_TxBUFF();

    //double INSAMPLE_I2S_L[1024];
    //double INSAMPLE_I2S_R[1024];

    //double OUTSAMPLE_I2S_L[1024];
    //double OUTSAMPLE_I2S_R[1024];

    /*for (int i = 2048; i < 4096; i ++){

                      if (i % 2 == 0){ // L Samples

                          INSAMPLE_I2S_L[i-(i >> 1)-1024] = (double) (I2S1_RxBUFF[i] << ADC_COMPENSATOR) * (0.01 * (double) volumeCurrent) * inputCompensator;

                    } else if ( i % 2 == 1){ // R Samples

                         INSAMPLE_I2S_R[i-1-((i-1)>>1)-1024] = (double) (I2S1_RxBUFF[i] << ADC_COMPENSATOR) * (0.01 * (double) volumeCurrent) * inputCompensator;

                    }
                }

                     arm_biquad_cascade_df2T_f64(&audioStream_L, INSAMPLE_I2S_L, OUTSAMPLE_I2S_L, 1024);
                     arm_biquad_cascade_df2T_f64(&audioStream_R, INSAMPLE_I2S_R, OUTSAMPLE_I2S_R, 1024);*/

                     for (int i = 2; i < 4; i ++){

                            I2S1_TxBUFF[i] = I2S1_RxBUFF[i];

                      /* if (i % 2 == 0) { // L Samples

                           I2S1_TxBUFF[i] = (int)OUTSAMPLE_I2S_L[i-(i >> 1)-1024];

                     } else if ( i % 2 == 1) { // R Samples

                           I2S1_TxBUFF[i] = (int)OUTSAMPLE_I2S_R[i-1-((i-1)>>1)-1024];

                     }*/
                }
                     DMA1_Stream1->CR |= DMA_SxCR_EN;

}

void MUTE(uint8_t INPUT_AUDIO_SELECTOR) {


    if((INPUT_AUDIO_SELECTOR == INPUT_INLINE) | (INPUT_AUDIO_SELECTOR == INPUT_BLUETOOTH))

             SPI1->CR1 |= SPI_CR1_CSUSP;
             while (!((SPI1->CR1) & (SPI_CR1_CSTART)));
             DMA1_Stream0->CR &= ~DMA_SxCR_EN;
             DMA1_Stream1->CR &= ~DMA_SxCR_EN;
             SPI1->CR1 &= ~SPI_CR1_SPE;
}

void UNMUTE(uint8_t INPUT_AUDIO_SELECTOR) {


    if((INPUT_AUDIO_SELECTOR == INPUT_INLINE) | (INPUT_AUDIO_SELECTOR == INPUT_BLUETOOTH))

             DMA1_Stream0->CR |= DMA_SxCR_EN;
             DMA1_Stream1->CR |= DMA_SxCR_EN;
             SPI1->CR1 |= SPI_CR1_SPE;
             SPI1->CR1 |= SPI_CR1_CSTART;
}