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


//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
						// (moze utrudnic detekcje przez glosnik - mikrofon)


float * DelayLine;

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[8];
float q2[8];
float r[8];
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) */
void	GOERTZEL(void)
{
	int i, row, col, peak_count, max_index;
	float q0, maxval, t;
	bool dtmf_ok;

	if (sample_count < 1230)
    {
        sample_count++;
        for (i=0; i<8; 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<8; 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<4; i++)
    	{
        	if ( r[i] > maxval )
	        {
    	        maxval = r[i];
        	    row = i;
        	}
    	}

    	// Najwiekszy w kolumnie:
		col = 4;
		maxval = 0;
    	for (i=4; i<8; i++)
    	{
        	if ( r[i] > maxval )
        	{
	            maxval = r[i];
    	        col = i;
        	}
	    }

	    // Minimalny poziom sygnalu
        if ((r[row] > 10) && (r[col] > 10))
		{
			dtmf_ok = 1;
			if ( r[col] > r[row] )
        	{
            	// Sprawdzenie "Normal twist"
            	max_index = col;
            	if ( r[row] < (r[col] * 0.158 ) )    // twist > 8dB, error
                	dtmf_ok = 0;
	        }
        	else
        	{
            	// Sprawdzenie "Reverse twist"
            	max_index = row;
            	if ( r[col] < (r[row] * 0.158 ) )    // twist > 8db, error
                	dtmf_ok = 0;
	        }

#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
            peak_count = 0;
		    for ( i=0; i<8; i++ )
        	{
            	if ( r[i] > t )
                	peak_count++;
        	}
        	if ( peak_count > 2 )		// wiecej niz 2 czestotliwosci - error
      	      dtmf_ok = 0;

	        if (dtmf_ok)
	        {
	        	if (col == 7)			// 4-ta kolumna (kody: ABCD - nieuzywane)
	        	{
	        		Leds |= FLG8;
	        		Leds |= row;
	        	}
	        	else
					switch (row) {
						case 0: Leds |= (col-3); break;	// 1-3
						case 1: Leds |= col; break;	// 4-6
						case 2: Leds |= (col+3); break;	// 7-9
						case 3: switch (col) {
									case 4: Leds |= 0x0c; break;	// *
									case 5: Leds |= 0; break;		// 0
									case 6: Leds |= 0x0f; break;	// #
						}
								break;
	        		}
				Leds |= FLG9;	// flaga detekcji tonu
	        }
		}
		*(int*) IOFLAG = Leds;	// zapalenie Led
    }
}



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

	/* Perform AD1836/AD1852/SPDIF Audio Processing Here */

// Wejscie: MIC IN, left&right (Left_Channel_In1, Right_Channel_In1)
// Wyjscie: CH1 LINE OUT, left&right (Left_Channel_Out0, Right_Channel_Out0) (EAR)
#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
	Right_Channel_Out1 = 0.5 * sinf(phase);				// sinus
	Left_Channel_Out1 = -Right_Channel_Out1;			// sinus + 180st
	if ((phase < 1.57079633) && (phase > -1.57079633))	// trojkat
		Left_Channel_Out2 = 0.3 * phase;
	else
		if (phase > 0)
			Left_Channel_Out2 = -0.3 * phase + 0.9424778;
		else
			Left_Channel_Out2 = -0.3 * phase - 0.9424778;
	if (phase > 0)										// prostokat
		Right_Channel_Out2 = 0.5;
	else
		Right_Channel_Out2 = -0.5;
	phase = phase + freq_ctrl;							// poprawka fazy
	if (phase > 3.141592654) phase = phase - 2*3.141592654;
	if ((PushB & 0x8) && (freq_ctrl < 1)) freq_ctrl += 0.000001;
	if ((PushB & 0x4) && (freq_ctrl > 0.0001)) freq_ctrl -= 0.000001;
#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;");
}