#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 512
#define I 1
#define NH 21
#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;
	}
}

/*

// 1. to 0-4800
DATA h1[NH] = {
	   13,     58,    174,    401,    777,   1311,   1969,   2673,   3306,
	 3748,   3907,   3748,   3306,   2673,   1969,   1311,    777,    401,
	  174,     58,     13
};

// 2. 5000-5500
DATA h2[NH] = {
     1913,  -7536,  -1914,   -389,     84,    119,    -67,   -194,   -116,
       87,    199,     87,   -116,   -194,    -67,    119,     84,   -389,
    -1914,  -7536,   1913
};


// 3. 5500-6000
DATA h3[NH] = {
	 5388,  -4079,  -2986,  -2035,  -1211,   -730,   -634,   -657,   -516,
	 -227,    -81,   -227,   -516,   -657,   -634,   -730,  -1211,  -2035,
	-2986,  -4079,   5388
};

// 4. 6000-6500
DATA h4[NH] = {
	 3867,   6140,  -3004,   1204,   -682,    526,    -56,    -71,   -223,
	   78,    195,     78,   -223,    -71,    -56,    526,   -682,   1204,
	-3004,   6140,   3867
};

// 6500-7000
DATA h5[NH] = {
	 -626,   7957,    668,   -127,   -114,    138,    160,    -92,   -189,
	   32,    202,     32,   -189,    -92,    160,    138,   -114,   -127,
	  668,   7957,   -626
};

// 7000-7500
DATA h6[NH] = {
	-4748,   5156,   3199,   1717,    867,    690,    585,    197,    -57,
	  114,    306,    114,    -57,    197,    585,    690,    867,   1717,
	 3199,   5156,  -4748
};

// 7500-8000
DATA h7[NH] = {
	-4750,  -5153,   3196,  -1715,    868,   -694,    588,   -199,    -56,
	 -115,    307,   -115,    -56,   -199,    588,   -694,    868,  -1715,
	 3196,  -5153,  -4750
};

// 8000-8500
DATA h8[NH] = {
	 -627,  -7958,    671,    127,   -111,   -141,    158,     93,   -191,
	  -29,    197,    -29,   -191,     93,    158,   -141,   -111,    127,
	  671,  -7958,   -627
};

// 8500-9000
DATA h9[NH] = {
	 3866,  -6140,  -3003,  -1203,   -683,   -528,    -57,     73,   -220,
	  -78,    193,    -78,   -220,     73,    -57,   -528,   -683,  -1203,
	-3003,  -6140,   3866
};

// 9000-9500
DATA h10[NH] = {
	 5494,   4159,  -3044,   2075,  -1236,    747,   -649,    671,   -525,
	  228,    -79,    228,   -525,    671,   -649,    747,  -1236,   2075,
	-3044,   4159,   5494
};

// 9500-10000
DATA h11[NH] = {
	 1915,   7540,  -1914,    388,     84,   -116,    -63,    196,   -117,
	  -94,    191,    -94,   -117,    196,    -63,   -116,     84,    388,
	-1914,   7540,   1915
};

// 10000-10500
DATA h12[NH] = {
	-2735,   7089,   2347,    648,    473,     41,   -111,    215,    -79,
	 -112,    207,   -112,    -79,    215,   -111,     41,    473,    648,
	 2347,   7089,  -2735
};

// 10500-11000
DATA h13[NH] = {
	-6161,  -3152,   2605,  -2132,   1642,  -1160,    806,   -646,    683,
	 -797,    851,   -797,    683,   -646,    806,  -1160,   1642,  -2132,
	 2605,  -3152,  -6161
};

// 11000-11500
DATA h14[NH] = {
	-3310,  -6609,   2859,  -1233,    453,    -53,   -113,    103,     14,
	 -142,    195,   -142,     14,    103,   -113,    -53,    453,  -1233,
	 2859,  -6609,  -3310
};

// 11500-12000
DATA h15[NH] = {
	 1173,  -7850,   -990,   -178,   -140,    196,   -185,     94,     40,
	 -157,    205,   -157,     40,     94,   -185,    196,   -140,   -178,
	 -990,  -7850,   1173
};

// 12000-12500
DATA h16[NH] = {
	 6790,   2119,  -1918,   1768,  -1638,   1478,  -1300,   1113,   -935,
	  808,   -763,    808,   -935,   1113,  -1300,   1478,  -1638,   1768,
	-1918,   2119,   6790
};

// 12500-13000
DATA h17[NH] = {
	 4929,   4972,  -3127,   1992,  -1276,    808,   -476,    236,    -77,
	  -28,     55,    -28,    -77,    236,   -476,    808,  -1276,   1992,
	-3127,   4972,   4929
};

// 13000-13500
DATA h18[NH] = {
	 1344,   7774,  -1452,    369,   -147,     56,     15,    -91,    146,
	 -179,    190,   -179,    146,    -91,     15,     56,   -147,    369,
	-1452,   7774,   1344
};

// 13500-14000
DATA h19[NH] = {
	-7452,  -1001,    971,   -946,    939,   -948,    960,   -970,    979,
	 -986,    989,   -986,    979,   -970,    960,   -948,    939,   -946,
	  971,  -1001,  -7452
};

// 14000-15000
DATA h20[NH] = {
	-7452,  -1001,    971,   -946,    939,   -948,    960,   -970,    979,
	 -986,    989,   -986,    979,   -970,    960,   -948,    939,   -946,
	  971,  -1001,  -7452
};

DATA* hArr1[4] = { h1, h2, h3, h4 };
DATA* hArr2[4] = { h5, h6, h7, h8 };
DATA* hArr3[4] = { h9, h10, h11, h12 };
DATA* hArr4[4] = { h13, h14, h15, h16 };
DATA* hArr5[4] = { h17, h18, h19, h20 };
DATA** hArr[5] = { hArr1, hArr2, hArr3, hArr4, hArr5 };

DATA in1b[W_LEN * 2] = {0};
DATA in2b[W_LEN * 2] = {0};
DATA* rin = in1b;
DATA* lin = in2b;

DATA db[NH + 2];  */

/*extern ushort fir(DATA *x, DATA *h, DATA *r, DATA *dbuffer, ushort nx, ushort nh);
int counter = 0;
void Equalizer() {

	/// 1 time
	int i = 0;
	for (i = 0; i < W_LEN; ++i) {
		rin[i] = buf_in_right_1[i];
		lin[i] = buf_in_left_1[i];
	}

	fir(rin, hArr[counter / 5][0], rin, db, W_LEN, NH);
	fir(lin, hArr[counter / 5][0], lin, db, W_LEN, NH);

	for (i = 0; i < W_LEN; ++i) {
		buf_out_right_1[i] = rin[i];
		buf_out_left_1[i] = lin[i];
	}


	// 2 time
	for (i = 0; i < W_LEN; ++i) {
		rin[i] = buf_in_right_1[i];
		lin[i] = buf_in_left_1[i];
	}

	fir(rin, hArr[counter / 5][1], rin, db, W_LEN, NH);
	fir(lin, hArr[counter / 5][1], lin, db, W_LEN, NH);
	for (i = 0; i < W_LEN; ++i) {
		buf_out_right_1[i] += rin[i];
		buf_out_left_1[i] += lin[i];
	}


	// 3 time
	for (i = 0; i < W_LEN; ++i) {
		rin[i] = buf_in_right_1[i];
		lin[i] = buf_in_left_1[i];
	}

	fir(rin, hArr[counter / 5][2], rin, db, W_LEN, NH);
	fir(lin, hArr[counter / 5][2], lin, db, W_LEN, NH);
	for (i = 0; i < W_LEN; ++i) {
		buf_out_right_1[i] += rin[i];
		buf_out_left_1[i] += lin[i];
	}


	// 4 time
	for (i = 0; i < W_LEN; ++i) {
		rin[i] = buf_in_right_1[i];
		lin[i] = buf_in_left_1[i];
	}

	fir(rin, hArr[counter / 5][3], rin, db, W_LEN, NH);
	fir(lin, hArr[counter / 5][3], lin, db, W_LEN, NH);
	for (i = 0; i < W_LEN; ++i) {
		buf_out_right_1[i] += rin[i];
		buf_out_left_1[i] += lin[i];
	}

	counter = ++counter % 20;
}
*/

Int16 in1b[W_LEN] = {0};
Int16 in2b[W_LEN] = {0};
DATA* rin = in1b;
DATA* lin = in2b;


//DATA in1b[W_LEN * 2] = {0};
//DATA in2b[W_LEN * 2] = {0};
//DATA* rin = in1b;
//DATA* lin = in2b;


//struct Q15 {
//	int num;
//	static const int n = 15;
//};

float min(float num1, float num2, float num3) {
	if(num1 < num2 && num1 < num3) return num1;
	else if(num2 < num3) return num2;
	else return num3;
}
//1, 0.5, 1, -0.5




##################In this part I need to use Q15 format instead of short and float##############################


void compexp(short CT, short CS, short ET, short ES) {
//void compexp(0x7333, 0x7333, 0x7333, 0x7333) {
	float tav = 0.01;
	float at = 0.03;
	float rt = 0.003;
//	int delay = 150;
	int xrms = 0;
	int g = 1;

	DATA buffer[150] = {0};

	int i;
	for (i = 0; i < W_LEN; ++i) {
		rin[i] = buf_in_right_1[i];
		lin[i] = buf_in_left_1[i];
	}

	for(i = 0; i < W_LEN; ++i) {
		xrms = (1 -tav) * xrms + tav * pow(rin[i], 2);
		float X = 10 * log(xrms);
		float G = min(0, CS * (CT - X), ES * (ET - X));
		int f = pow (10, G / 20);
		float coeff;

		if (f < g) {
			coeff = at;
		} else {
			coeff = rt;
		}

		g = (1 - coeff) * g + coeff * f;
		buf_out_right_1[i] = g * buffer[149];
		buf_out_left_1[i]  = g * buffer[149];

		buffer[0] = buf_in_left_1[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) {
			/* äîáàâèòü ôóíêöèþ ýôôåêòà */
//			Equalizer();
			compexp(1, 0.5, 1, -0.5);
			buf_out_left_1 = buf_in_left_1;
			buf_out_right_1 = buf_in_right_1;
			effect_flag = NO_EFFECT_FLAG;
		}
	}
}