#include "C5515.h"
#include "gpio.h"
#include "i2c.h"
#include "i2s.h"

#include "TMS320.h"
#include "stdio.h"
#include "math.h"
#include "dsplib.h"
#include "dsplib_c.h"

#include "aic3204.h"

#define W_LEN 4096
#define EFFECT_FLAG 1
#define NO_EFFECT_FLAG 0
#define IER0 (*(unsigned int*)0x00)
#define IVPD (*(unsigned int*)0x000049)
#define IVPH (*(unsigned int*)0x00004A)

extern unsigned int VECSTART;

/*********Буферы левого и правого канала************/
Int16 b1[W_LEN] = { 0 };
Int16 b2[W_LEN] = { 0 };
Int16 b3[W_LEN] = { 0 };
Int16 b4[W_LEN] = { 0 };

Int16 b5[W_LEN] = { 0 };
Int16 b6[W_LEN] = { 0 };
Int16 b7[W_LEN] = { 0 };
Int16 b8[W_LEN] = { 0 };

Int16* buf_in_left_1 = b1;
Int16* buf_in_right_1 = b2;
Int16* buf_out_left_1 = b3;
Int16* buf_out_right_1 = b4;

Int16* buf_in_left_2 = b5;
Int16* buf_in_right_2 = b6;
Int16* buf_out_left_2 = b7;
Int16* buf_out_right_2 = b8;

Int16* change;

Int16 buf_index = 0;

Int16 effect_flag = NO_EFFECT_FLAG;

/* Обработчик прерывания по передаче */
interrupt void i2s2_tx_isr()
{
    I2S2_W0_MSW_W = buf_out_left_2[buf_index];
    I2S2_W1_MSW_W = buf_out_right_2[buf_index];

    while ((Rcv & I2S2_IR) == 0);

    buf_in_left_2[buf_index] = I2S2_W0_MSW_R;
    buf_in_right_2[buf_index] = I2S2_W1_MSW_R;

    buf_index++;

    if (buf_index >= W_LEN) {
        change = buf_in_left_2;
        buf_in_left_2 = buf_in_left_1;
        buf_in_left_1 = change;

        change = buf_out_left_2;
        buf_out_left_2 = buf_out_left_1;
        buf_out_left_1 = change;

        change = buf_in_right_2;
        buf_in_right_2 = buf_in_right_1;
        buf_in_right_1 = change;

        change = buf_out_right_2;
        buf_out_right_2 = buf_out_right_1;
        buf_out_right_1 = change;

        buf_index = 0;
        effect_flag = EFFECT_FLAG;
    }
}


void allpass(Int16 * input, Int16 * output, float c){

    float oldy = 0;
    float oldx = 0;
    int i = 0;

    for(i=0; i < W_LEN; ++i)
    {
      output[i] = c * input[i] + oldx - c * oldy;
      oldx = input[i];
      oldy = output[i];
    }
}

void main(void) {

    /* Initialize BSL */
    c5515_init();
    /* Configure Parallel Port */
    SYS_EXBUSSEL &= ~0x7000;
    SYS_EXBUSSEL |= 0x1000;  // Configure Parallel Port for I2S2

    /* Configure Serial Port */
    SYS_EXBUSSEL &= ~0x0C00;
    SYS_EXBUSSEL |= 0x0400; // Serial Port mode 1 (I2S1 and GP[11:10]).
    c5515_GPIO_init();
    c5515_GPIO_setDirection(GPIO10, GPIO_OUT);
    c5515_GPIO_setOutput(GPIO10, 1); // Take AIC3201 chip out of reset
    I2C_init(); // Initialize I2C

    asm(" BSET INTM");
    unsigned long int vector;
    vector = (unsigned long int) &VECSTART;
    vector = vector >> 8;
    IVPD = (unsigned short) vector;
    IVPH = (unsigned short) vector;
    IER0 |= 0x4000;
    asm(" BCLR INTM");

    /* I2S settings */
    I2S2_SRGR = 0x0015;
    I2S2_ICMR = 0x0028; // Enable interrupts
    I2S2_CR = 0x8012; // 16-bit word, Master, enable I2C

    aic3204_stereo_in1();

    while (1) {

        if (effect_flag == EFFECT_FLAG) {
            /* добавить функцию эффекта */
//          effect_flag = NO_EFFECT_FLAG;
//          buf_out_left_1 = buf_in_left_1;
//          buf_out_right_1 = buf_in_right_1;
            float c1 = 0.1;
            float c2 = 0.09;
            allpass(buf_in_left_1, buf_out_left_1, c1);
//          allpass(buf_in_left_1, buf_out_right_1, c2);
            buf_out_right_1 = buf_in_left_1;
        }
    }
}