//Autorzy: Damian Dybek i Karol Draszawka

#include "ADDS_21161_EzKit.h"
#include <def21161.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>

#define NUM_OF_FRAMES 50		
#define NUM_OF_LPC_COEF 10

float euclidean_dist(float*, float*);
float odleglosc_sygnal_wzorzec(float*, float*);


float ramka1[160];
//float ramka2[160];	//gdy z nakladkowaniem

//z nakladkowaniem 50%: gdy sygnal ma trwac 1s to dlugosc tablic= 990, gdy 2s to 1990, 3s to 2990
//bez nakladkowania:    gdy sygnal ma trwac 1s to dlugosc tablic= 500, gdy 2s to 1000, 3s to 1500
float dm wzorzec1[500];
float dm wzorzec2[500];
float dm wzorzec3[500];

//zmienne pomocnicze do dtw
float dist[NUM_OF_FRAMES][NUM_OF_FRAMES];
float pomoc1,pomoc2;
int i,j;
float syg2[NUM_OF_FRAMES][NUM_OF_LPC_COEF];
float ref2[NUM_OF_FRAMES][NUM_OF_LPC_COEF];

float test[500];
float lpc_coefs[10];

float result1;
float result2;
float result3;
int zwyciezca;
float granica = 11.0; //okreslenie od jakiej odleglosci uznawac ze wzorzec pasuje

float sample;

int sample_count = 0;
int frame_count = 0;
int stan = 0;
int gdzie = 0;
int n = 0;
int i = 0;
int j=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;
}


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

if (PushB ==1)//przejdz w stan nagrywania wzorcow
	{
		stan = 1;
		gdzie = 1;
		n = 0;
		frame_count=0;
		
		Leds = 0x0000ff20;			//OXXXXX
		*(int*) IOFLAG = Leds;	
	}
if (PushB ==2) //przejdz w stan nagrywania wzorca2
	{
		stan = 1;
		gdzie = 2;
		n = 0;
		frame_count=0;
		
		Leds = 0x0000ff10;			//XOXXXX
		*(int*) IOFLAG = Leds;
	}
if (PushB ==4) //przejdz w stan nagrywania wzorca3
	{
		stan = 1;
		gdzie = 3;
		n = 0;
		frame_count=0;
		
		Leds = 0x0000ff08;			//XXOXXX
		*(int*) IOFLAG = Leds;
	}

if (PushB ==8) //przejdz w stan testowania
	{
		stan = 2;
		n = 0;
		frame_count=0;
		
		Leds = 0x0000ff01;			//XXXXXO
		*(int*) IOFLAG = Leds;
	}
	
///////////////////////////////////////////////////////////	
if (stan==1)		//stan nagrywania wzorca
{
	//decymacja (co 6 probka, tak by z 48 ksamp/s bylo 8ksamp/s)
	sample_count++;
	if(sample_count==5){
		sample_count=0;
		ramka1[n]=Right_Channel_In1;
		//printf("ramka1[%d]= %f\n", n , ramka1[n]);
		n=n+1;
		if(n==160){
			n=0;
			frame_count=frame_count+1;
			
			lpc(ramka1,lpc_coefs);
					
			switch (gdzie) {
				case 1: for(i=0;i<10;i++) wzorzec1[frame_count*10+i]=lpc_coefs[i];
						//miganie diody
						if(frame_count%10==0){
							Leds ^= 0x00000020;			
							*(int*) IOFLAG = Leds;
						}					
						break;
										
				case 2: for(i=0;i<10;i++) wzorzec2[frame_count*10+i]=lpc_coefs[i];
						//miganie diody
						if(frame_count%10==0){
							Leds ^= 0x00000010;			
							*(int*) IOFLAG = Leds;
						}
						break;
				
				case 3: for(i=0;i<10;i++) wzorzec3[frame_count*10+i]=lpc_coefs[i];
						//miganie diody
						if(frame_count%10==0){
							Leds ^= 0x0000008;			
							*(int*) IOFLAG = Leds;
						}
						break;				
			}			
			
			//printf("frame_count %d\n", frame_count);
			
			
			if(frame_count==50){
				frame_count=0;
				//printf("frame_count %d\n", frame_count);
				stan=0;			//przejdz do nicnierobienia
				
				Leds = 0x0000ff00;			//XXXXXX
				*(int*) IOFLAG = Leds;
			}
		}
	}
}

///////////////////////////////////////////////////////////	
if (stan==2)		//stan testowania
{
	//decymacja (co 6 probka, tak by z 48 ksamp/s bylo 8ksamp/s)
	sample_count++;
	if(sample_count==5){
		sample_count=0;
		ramka1[n]=Right_Channel_In1;
		n=n+1;
		if(n==160){
			n=0;
			lpc(ramka1,lpc_coefs);			
			for(i=0;i<10;i++){
				test[frame_count*10+i]=lpc_coefs[i];
			}
			
			frame_count=frame_count+1;
			//miganie diody
			if(frame_count%10==0){
				Leds ^= 0x00000001;			
				*(int*) IOFLAG = Leds;
			}
			if(frame_count==50){
				frame_count=0;
				stan=0;			//przejdz do nicnierobienia
				
				//printf("2");
				
				result1=odleglosc_sygnal_wzorzec(test,wzorzec1);	//liczenie odleglosci
				result2=odleglosc_sygnal_wzorzec(test,wzorzec2);	//liczenie odleglosci
				result3=odleglosc_sygnal_wzorzec(test,wzorzec3);	//liczenie odleglosci
				
				
				//okreslenie zwyciezcy i zapalenie odpowiedniej diody
				if(result1<result2){		
					if(result1<result3){
						if(result1>granica){zwyciezca=0;}else{
						zwyciezca=1;}
					}else{
						if(result3>granica){zwyciezca=0;}else{
						zwyciezca=3;}
					}
				}else{
					if(result2<result3){
						if(result2>granica){zwyciezca=0;}else{
						zwyciezca=2;}
					}else{
						if(result3>granica){zwyciezca=0;}else{
						zwyciezca=3;}
					}
				}
				
				
				//zapalenie odpowiedniej diody
				switch(zwyciezca){
					case 0: Leds = 0x0000ff00;			//XXXXXX - wypowiedz nie pasuje do zadnego ze wzorcow
							*(int*) IOFLAG = Leds;
							break;
					case 1: Leds = 0x0000ff20;			//OXXXXX
							*(int*) IOFLAG = Leds;
							break;
					case 2: Leds = 0x0000ff10;			//XOXXXX
							*(int*) IOFLAG = Leds;
							break;
					case 3: Leds = 0x0000ff08;			//XXOXXX
							*(int*) IOFLAG = Leds;
							break;
				}
				
				printf("result1 = %f\n", result1);
				printf("result2 = %f\n", result2);
				printf("result3 = %f\n\n", result3);
			}
		}
	}
}


///////////////////////////////////////////////////////////	
if (stan==0)			//nicnierobienie
	{
		//Left_Channel_Out0=Left_Channel_In1;
		//Right_Channel_Out0=Right_Channel_In1;
	}
///////////////////////////////////////////////////////////

//  Ledy i przyciski (FLAGS):

asm("dm(_PushB) = FLAGS;");
/*
Leds = *(int*) IOFLAG;
Leds &= ~(FLG4|FLG5|FLG6|FLG7);
Leds |= PushB;
*(int*) IOFLAG = Leds;
*/

//	Transmit_Samples();
}



float euclidean_dist(float* ramka1, float* ramka2)
{
	int u;
    float output = 0.0;
    for( u=0; u<NUM_OF_LPC_COEF; u++ ) 
    {
		output += (ramka1[u]-ramka2[u])*(ramka1[u]-ramka2[u]);
	}

    return( sqrt(output));
    //return(output);
}



float odleglosc_sygnal_wzorzec(float* syg, float* ref)
{
	
	for (i=0;i<NUM_OF_FRAMES; i++) {
		for (j=0; j<NUM_OF_LPC_COEF; j++){
			syg2[i][j]=syg[NUM_OF_LPC_COEF*i+j];
			ref2[i][j]=ref[NUM_OF_LPC_COEF*i+j];
		}
	}
	
	//distance matrix calculate
	for(i=0; i<NUM_OF_FRAMES; i++){
		for(j=0; j<NUM_OF_FRAMES; j++){
			dist[i][j]=euclidean_dist(syg2[i],ref2[j]);
			//dist[i][j]=abs_dist(syg2[i],ref2[j]);
		}
	}
	
	//accumulated distance matrix build
	for(j=1; j<NUM_OF_FRAMES; j++){
		dist[0][j]=dist[0][j-1]+dist[0][j];
	}
	for(i=1; i<NUM_OF_FRAMES; i++){
		dist[i][0]=dist[i-1][0]+dist[i][0];
	}
	for(i=1; i<NUM_OF_FRAMES; i++){
		for(j=1; j<NUM_OF_FRAMES; j++){
			//dist[i][j]=dist[i][j]+min(dist[i-1][j],min(dist[i-1][j-1],dist[i][j-1]));
			if(dist[i-1][j]<dist[i-1][j-1]){
				if(dist[i-1][j]<dist[i][j-1]){
					dist[i][j]=dist[i][j]+dist[i-1][j];
				}else{
					dist[i][j]=dist[i][j]+dist[i][j-1];
				}
			}else{
				if(dist[i-1][j-1]<dist[i][j-1]){
					dist[i][j]=dist[i][j]+dist[i-1][j-1];
				}else{
					dist[i][j]=dist[i][j]+dist[i][j-1];
				}
			}
		}
	}
	//printf("dist= %f",dist[NUM_OF_FRAMES-1][NUM_OF_FRAMES-1]);
	return dist[NUM_OF_FRAMES-1][NUM_OF_FRAMES-1];
}



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;
//probki = (float *) 0x00000800;
//	real = (float *) 0x00000800;
//	imag = (float *) 0x00000800;
//	moddft = (float *) 0x00000800;
	
	for (;;)
		asm("idle;");
}