#include "ADDS_21161_EzKit.h"
#include <def21161.h>
#include <signal.h>
#include <math.h>
#include <filter.h> 
#include <stdio.h>
//#define N 2048 
//define FIR		// Interpolowany filtr FIR:
				// Wejscie: CH1 LINE IN, right (Right_Channel_In0)
				// Wyjscie: CH4 LINE OUT, left & right (Left_Channel_AD1852, Right_Channel_AD1852)

//#define FUNGEN	// Generator funkcji:
				// Sinus: CH2 LINE OUT, left(phase shift 180 deg) & right (Left_Channel_Out1, Right_Channel_Out1)
				// Trojkat: CH3 LINE OUT, left (Left_Channel_Out2)
				// Prostokat: CH3 LINE OUT, right (Right_Channel_Out2)
				// Flag2, Flag3 - regulacja czestotliwosci

//#define DELLINE	// Simple stereo digital delay line:
				// Wejscie: MIC IN, left & right (Left_Channel_In1, Right_Channel_In1)
				// Wyjscie: CH1 LINE OUT, left & right (Left_Channel_Out0, Right_Channel_Out0) (EAR)

#define DTMF	// Dekoder DTMF (algorytm Goertzel'a):
				// Wejscie: MIC IN, right (Right_Channel_In1)
				// Wyjscie: Flag9 - wykryto tony
				//          Flag7-4 - wykryta cyfra 0-9 (binarnie),  # = "1111",  * = "1100"

//#define PERFECT_DTMF	// Filtrowanie zaklocen i falszywych sygnalow
int u;
int p=0;						// (moze utrudnic detekcje przez glosnik - mikrofon)
float input[64];
int i=0;
int dzwieki[60];
float * DelayLine;
int numer[100];
float wartosc[100];
float Test_Wave[256];	// do testowania funkcji PLOT (DELLINE)
int Index1 = 0;

float phase = 0;		// faza generatora sygnalowego
float freq_ctrl = 0.05;	// czestotliwosc generatora sygnalowego

/*
   Filtr interpolowany (x5):
   fsample = 48 kHz
   Passband < 3 kHz
   Stopband = 3.5-6 kHz
*/
int coef_ifir[36] = {112,1117,427,-753,422,476,-1091,544,899,-1729,641,1750,-2922,710,3962,-6558,
746,31245,31245,746,-6558,3962,710,-2922,1750,641,-1729,899,544,-1091,476,422,-753,427,1117,112};

/*
   Filtr Image Rejection:
   fsample = 48 kHz
   Passband < 3 kHz
   Stopband > 6 kHz
*/
int coef_ir[32] = {375,661,865,815,374,-475,-1564,-2529,-2883,-2154,-58,3355,7650,12065,15699,
17751,17751,15699,12065,7650,3355,-58,-2154,-2883,-2529,-1564,-475,374,815,865,661,375};

float taps_ifir[176];	// linia opozniajaca
float taps_ir[32];		// linia opozniajaca
int tap_ifir = 0;		// akt. pozycja wpisywania nowych probek
int tap_ir = 0;			// akt. pozycja wpisywania nowych probek
float result;
float sample;


/*
	Wspolczynnkiki do algorytmu Goertzel'a
*/
//float goertzel_coef[8] = {1.99155133, 1.98957126, 1.98738353, 1.98498837,
//						  1.97497552, 1.96991047, 1.96243767, 1.95414554};
float q1[60];
float q2[60];
float r[60];
int sample_count = 0;

int Index  = 0;

int PushB = 0;
int Leds = 0;



// Obsluga przerwan przyciskow:
void	Process_IRQ_0(int sig_int)
{
	*(int*) IOFLAG ^= FLG8;
}

void	Process_IRQ_1(int sig_int)
{
	*(int*) IOFLAG ^= FLG9;
}

void	Process_IRQ_2(int sig_int)
{
	*(int*) IOFLAG ^= FLG9|FLG8;
}



/* Filtr FIR (interpolowany) - kaskada dwoch filtrow */
void	IFIR(void)
{
	int i, tap;

	/*  Filtr interpolowany (x5) */
	tap = tap_ifir;
	taps_ifir[tap] = sample / 60000;
	result = 0;
	for (i=0; i<36; i++)
	{
		result += taps_ifir[tap] * coef_ifir[i];
		tap += 5;
	    if (tap > 175) tap -= 176;
	}
	if (tap_ifir == 0)
		tap_ifir = 175;
	else
		tap_ifir--;

	/* Filtr "image rejection" */
	tap = tap_ir;
	taps_ir[tap] = result / 100000;
	result = taps_ir[(tap++)&0x1F] * coef_ir[0];
	result += taps_ir[(tap++)&0x1F] * coef_ir[1];
	result += taps_ir[(tap++)&0x1F] * coef_ir[2];
	result += taps_ir[(tap++)&0x1F] * coef_ir[3];
	result += taps_ir[(tap++)&0x1F] * coef_ir[4];
	result += taps_ir[(tap++)&0x1F] * coef_ir[5];
	result += taps_ir[(tap++)&0x1F] * coef_ir[6];
	result += taps_ir[(tap++)&0x1F] * coef_ir[7];
	result += taps_ir[(tap++)&0x1F] * coef_ir[8];
	result += taps_ir[(tap++)&0x1F] * coef_ir[9];
	result += taps_ir[(tap++)&0x1F] * coef_ir[10];
	result += taps_ir[(tap++)&0x1F] * coef_ir[11];
	result += taps_ir[(tap++)&0x1F] * coef_ir[12];
	result += taps_ir[(tap++)&0x1F] * coef_ir[13];
	result += taps_ir[(tap++)&0x1F] * coef_ir[14];
	result += taps_ir[(tap++)&0x1F] * coef_ir[15];
	result += taps_ir[(tap++)&0x1F] * coef_ir[16];
	result += taps_ir[(tap++)&0x1F] * coef_ir[17];
	result += taps_ir[(tap++)&0x1F] * coef_ir[18];
	result += taps_ir[(tap++)&0x1F] * coef_ir[19];
	result += taps_ir[(tap++)&0x1F] * coef_ir[20];
	result += taps_ir[(tap++)&0x1F] * coef_ir[21];
	result += taps_ir[(tap++)&0x1F] * coef_ir[22];
	result += taps_ir[(tap++)&0x1F] * coef_ir[23];
	result += taps_ir[(tap++)&0x1F] * coef_ir[24];
	result += taps_ir[(tap++)&0x1F] * coef_ir[25];
	result += taps_ir[(tap++)&0x1F] * coef_ir[26];
	result += taps_ir[(tap++)&0x1F] * coef_ir[27];
	result += taps_ir[(tap++)&0x1F] * coef_ir[28];
	result += taps_ir[(tap++)&0x1F] * coef_ir[29];
	result += taps_ir[(tap++)&0x1F] * coef_ir[30];
	result += taps_ir[(tap++)&0x1F] * coef_ir[31];
	if (tap_ir == 0)
		tap_ir = 31;
	else
		tap_ir--;
}



/* Goertzel N=1230 (fsample = 48 kHz) */

float goertzel_coef[60] = 

{
1.99992385,
1.99991763,
1.99991117,
1.99989753,
1.99988291,
1.99986731,
1.99985074,
1.99983319,
1.99981467,
1.99979518,
1.99976411,
1.99974218,
1.99970745,
1.99967053,
1.99963142,
1.99959012,
1.99953164,
1.99948522,
1.99941991,
1.99935071,
1.99925873,
1.99918076,
1.99907781,
1.99894624,
1.99882990,
1.99868225,
1.99852583,
1.99833227,
1.99812677,
1.99790935,
1.99764626,
1.99736759,
1.99703546,
1.99668363,
1.99626960,
1.99583124,
1.99532095,
1.99473073,
1.99410550,
1.99338870,
1.99257084,
1.99164176,
1.99065809,
1.98947728,
1.98822641,
1.98674553,
1.98509230,
1.98334232,
1.98130570,
1.97895068,
1.97645676,
1.97348531,
1.97033856,
1.96663692,
1.96258533,
1.95801983,
1.95289375,
1.94715781,
1.94076023,
1.93346832,
//1.92538349
};



//float q1[60];
//float q2[60];
//float r[60];
//int sample_count = 0;
//int Index = 0;
//int PushB = 0;
//int Leds = 0;



// Obsluga przerwan przyciskow:
/*
void Process_IRQ_0(int sig_int)
{
*(int*) IOFLAG ^= FLG8;
}

void Process_IRQ_1(int sig_int)
{
*(int*) IOFLAG ^= FLG9;
}

void Process_IRQ_2(int sig_int)
{
*(int*) IOFLAG ^= FLG9|FLG8;
}

*/


/* Goertzel N=18000 (fsample = 48 kHz) */
//trzeba zmienic N= 18000 jak w pliku xls
void GOERTZEL(void)
{
	int i, row, col, peak_count, max_index;
	float q0, maxval, t;
	bool dtmf_ok;
		if (sample_count < 18000)
		{
		sample_count++;
		for (i=0; i<60; i++) // Realizacja jednego kroku algorytmu dla wszystkich czestotliwosci
			{
				q0 = goertzel_coef[i] * q1[i] - q2[i] + sample;
				q2[i] = q1[i];
				q1[i] = q0;
			}
		}
		else
		{
			// Obliczenie mocy skladowych i zerowanie pamieci dla nastepnego pomiaru

			for (i=0; i<60; i++)
				{
					r[i] = (q1[i] * q1[i]) + (q2[i] * q2[i]) - (goertzel_coef[i] * q1[i] * q2[i]);
					q1[i] = 0;
					q2[i] = 0;
				}
			sample_count = 0;

				// Interpretacja odczytu:

// Odczyt stanu LED:

				Leds = *(int*) IOFLAG;
				Leds &= ~(FLG4|FLG5|FLG6|FLG7|FLG8|FLG9);

// Najwiekszy w wierszu:
				row = 0;
				maxval = 0;
				for (i=0; i<60; i++)	
				{
				if ( r[i] > maxval )
					{
						maxval = r[i];
						row = i;
					}

					if (i==59)
						{
					numer[p]=row;
					wartosc[p]=maxval;
					p=p+1;
								}
				if (p==80 )
				{
			for (u=0; u<80; u++){
if (u%20==1) printf("  \n");

switch (numer[u])
{
//case 0: printf(" c1 \n", u); break; 
//case 1: printf(" cis1 \n", u); break; 

//case 2: printf(" d1 \n", u); break; 
//case 3: printf(" dis1 \n", u); break;
//case 4: printf(" e1 \n", u); break; 
//case 5: printf(" f1 \n", u); break; 
//case 6: printf(" fis1 \n", u); break; 
//case 7: printf(" g1 \n", u); break;
//case 8: printf(" gis1 \n", u); break; 
//case 9: printf(" a1 \n", u); break;
//case 10: printf(" b1  ", u); break; 
//case 11: printf(" h1  ", u); break;
//case 12: printf(" c2  ", u); break; 
//case 13: printf(" cis2  ", u); break;
case 14: printf(" d2  ", u); break; 
case 15: printf(" dis2  ", u); break; 
case 16: printf(" e2  ", u); break; 
case 17: printf(" f2  ", u); break;
case 18: printf(" fis2  ", u); break; 
case 19: printf(" g2  ", u); break;
case 20: printf(" gis2  ", u); break; 
case 21: printf(" a2  ", u); break; 
case 22: printf(" b2  ", u); break; 
case 23: printf(" h2  ", u); break; 
case 24: printf(" c3  ", u); break;
case 25: printf(" cis3 ", u); break; 
case 26: printf(" d3  ", u); break; 
case 27: printf(" dis3  ", u); break; 
case 28: printf(" e3  ", u); break;
case 29: printf(" f3  ", u); break; 
case 30: printf(" fis3  ", u); break;
case 31: printf(" g3  ", u); break; 
case 32: printf(" gis3  ", u); break; 
case 33: printf(" a3  ", u); break; 
case 34: printf(" b3  ", u); break;
case 35: printf(" h3  ", u); break;
case 36: printf(" c4  ", u); break; 
case 37: printf(" cis4  ", u); break; 
case 38: printf(" d4  ", u); break; 
case 39: printf(" dis4  ", u); break;
case 40: printf(" e4  ", u); break; 
case 41: printf(" f4  ", u); break;
case 42: printf(" fis4  ", u); break; 
case 43: printf(" g4  ", u); break; 
case 44: printf(" gis4  ", u); break; 
case 45: printf(" a4  ", u); break;
case 46: printf(" b4  ", u); break; 
/*case 47: printf(" h4  ", u); break; 
case 48: printf(" c5 \n", u); break; 
case 49: printf(" cis5 \n", u); break; 
case 50: printf(" d5 \n", u); break;
case 51: printf(" dis5 \n", u); break; 
case 52: printf(" e5 \n", u); break;
case 53: printf(" f5 \n", u); break; 
case 54: printf(" fis5 \n", u); break; 
case 55: printf(" g5 \n", u); break; 
case 56: printf(" gis5 \n", u); break;
case 57: printf(" a5 \n", u); break; 
case 58: printf(" b5 \n", u); break; 
case 59: printf(" h5 \n", u); break; 
case 60: printf(" c6 \n", u); break; 
*/
}
			}
p=12;				
				}
		//		for  (u=0; u<80; u++)
		//			switch (numer[u])
		//		
		//			case 
		//			
		//		printf("skala %f\n",x );
			//	p=0;
			
				}
		
				
				}
		


if (r[row] > 10)//nawyzej sie wstawi tu inna wartosc hehe

dtmf_ok = 1;





#ifndef PERFECT_DTMF		// Sprawdzenie "twist" i S/N
			dtmf_ok = 1;
#endif

			// sprawdzenie stosunku S/N
#ifdef PERFECT_DTMF
			if ( r[max_index] > 200 )
				t = r[max_index] * 0.200;
			else
				if ( r[max_index] > 60 )
					t = r[max_index] * 0.060;
				else
	        		t = r[max_index] * 0.020;
#else
//            t = r[max_index] * 0.200;
#endif
 
    }




// Obsluga przerwania portow kodeka (zmodyfikowana przez MK)
void	Process_Samples( int sig_int)
{
	Receive_Samples();

#ifdef DELLINE
	Left_Channel_Out0 = DelayLine[Index];
	DelayLine[Index++] = Left_Channel_In1;
	Right_Channel_Out0 = DelayLine[Index];
	DelayLine[Index++] = Right_Channel_In1;
	if (Index == 0x80000) Index = 0;

	Test_Wave[Index1++] = Left_Channel_In1;			// do testowania funkcji "Plot"
	Index1 &= 0x00FF;
#endif

//	Left_Channel_AD1852 = Left_Channel_SPDIF_rx;
//	Right_Channel_AD1852 = Right_Channel_SPDIF_rx;

//  Generator funkcji
#ifdef FUNGEN





#endif

//  Ledy i przyciski (FLAGS):
#ifndef DTMF
	asm("dm(_PushB) = FLAGS;");
	Leds = *(int*) IOFLAG;
	Leds &= ~(FLG4|FLG5|FLG6|FLG7);
	Leds |= PushB;
	*(int*) IOFLAG = Leds;
#endif

//  Filtr FIR
#ifdef FIR
	sample = Right_Channel_In0;
	IFIR();
	Left_Channel_AD1852 = Right_Channel_AD1852 = result;
#endif

//  Dekoder DTMF (Goertzel)
#ifdef DTMF
	sample = Right_Channel_In1;
	GOERTZEL();
	Left_Channel_Out0 = Right_Channel_Out0 = sample;
#endif

	Transmit_Samples();
}


void	main()
{
	/* Setup Interrupt edges and flag I/O directions */
	Setup_ADSP21161N();

	/* Setup SDRAM Controller */
	Setup_SDRAM();

	Setup_AD1836();
//	Init_AD1852_DACs();    // usuniete przez MK - konfiguracja DAC4 przez SPI konfliktuje z FLAG1
                           // użytym jako pushbutton

	Program_SPORT02_TDM_Registers();
	Program_SPORT02_DMA_Channels();

	interruptf(	SIG_SP0I,	Process_Samples);

#ifdef DTMF
	Leds = *(int*) IOFLAG;
	Leds &= ~(FLG4|FLG5|FLG6|FLG7|FLG8|FLG9);
	*(int*) IOFLAG = Leds;
#else
	interruptf( SIG_IRQ0,   Process_IRQ_0);
	interruptf( SIG_IRQ1,   Process_IRQ_1);
	interruptf( SIG_IRQ2,   Process_IRQ_2);
#endif

	*(int *) SP02MCTL |= MCE;

	DelayLine = (float *) 0x00200000;

	for (;;)
		asm("idle;");
}