/*
 *  Copyright 2010 by Spectrum Digital Incorporated.
 *  All rights reserved. Property of Spectrum Digital Incorporated.
 */

#include "usbstk5515.h"
#include "aic3204.h"
#include "stdio.h"        // For printf();
#include "usbstk5515_i2c.h"
#include "usbstk5515_emif.h"
#include "usbstk5515_gpio.h"
#include "usbstk5515_led.h"


#include "fdacoefs.h"
#include "tms320.h"
#include "Dsplib.h"


#define SIZE 51



Int16 before_left[480];
Int16 before_right[480];
Int16 after_left[480];
Int16 after_right[480];

Int16 after_Kron[60];

int i=0;

Int16 right;//zmienna do przechowywania probki  z lewego kanalu
Int16 left;//zmienna do przechowywania probki  z prawego kanalu

Int16 oright;//zmienna do przechowywania probki na wyjscie leweego kanalu
Int16 oleft;//zmienna do przechowywania probki  na wyjscie prawego kanalu

 Int16 dbuffer[SIZE + 2];//bufor opoznien
 Int16 * delay=&dbuffer[0];//wskaznik na bufor opoznien
 int NH=51;//ilosc wspolczynnikow filtru
 Int16 * xl=&left;//wskazniki na zmienne wejscia/wyjscia
 Int16 * xr=&right;
 Int16 * yl=&oleft;
 Int16 * yr=&oright;




 int actual_index=50;
 //int delay_index=0;

 Int16 Kronecker[SIZE];

 //JAKBY DSPLIB NIE DZIALALO TO PRZEKOPIOWAC WSPOLCZYNNIKI DO PLIKU MAIN.C

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  Testing Function                                                        *
 *                                                                          *
 * ------------------------------------------------------------------------ */


  Int16 FIR(Int16 input[], Int16 h[])
  {//filtr FIR w C DZIALA

	  	  long out=0;
	  	  int i=0;

	  for (i=0; i<SIZE; i++)
	     {
	        out += h[i]*(long)input[i];//filtracja
	     }

	  return(out>>15);
  }


void TEST_execute( Int16 ( *funchandle )( ), char *testname, Int16 testid )
{
    Int16 status;

    /* Display test ID */
    printf( "%02d  Testing %s...\n", testid, testname );

    /* Call test function */
    status = funchandle( );

    /* Check for test fail */
    if ( status != 0 )
    {
        /* Print error message */
        printf( "     FAIL... error code %d... quitting\n", status );

        /* Software Breakpoint to Code Composer */
        SW_BREAKPOINT;
    }
    else
    {
        /* Print error message */
        printf( "    PASS\n" );
    }
}

/* ------------------------------------------------------------------------ *
 *                                                                          *
 *  main( )                                                                 *
 *                                                                          *
 * ------------------------------------------------------------------------ */
extern Int16 fir_filter( Int16* input, Int16* h, Int16 *delay);

void main( void )
{
    /* Initialize BSL */
    USBSTK5515_init( );

    /* Initialise hardware interface and I2C for code */
        aic3204_hardware_init();

        /* Initialise the AIC3204 codec */
    	aic3204_init();


    printf( "\n***ALL Tests Passed***\n" );


    set_sampling_frequency_and_gain(48000,2);//ustawienie fs i wzmocnienia

   for (i=0;i<SIZE + 2;i++)//czyszczenie bufora opoznien
    {
    	dbuffer[i]=0;
    }


    i=0;

     for (i=0;i<SIZE;i++)//czyszczenie pozostalych buforow i tablicowanie delty Kroneckera
    {
    	Kronecker[i]=0;
    	before_left[i]=0;
    	after_left[i]=0;
    	before_right[i]=0;
    	after_right[i]=0;
    	after_Kron[i]=0;
    }

    Kronecker[0]=32767;//nadanie wartosci pierwszej probki delty Kroneckera


    //FIR DSPLIB
    /*
    while(1)
    {

    		//to wysyla na oba kanaly coś
    		aic3204_codec_read(xl, xr);//pobranie probki


   		    before_left[i]=left;//zapisanie probek wejsciowych do tablicy
    		//left=Kronecker[i];
    		before_right[i]=right;
    	    fir(xl,B,yl,delay,1,NH);//filtracja kanalu lewego
    	    fir(xr,B,yr,delay,1,NH);//filtracja kanalu prawego
    	    after_Kron[i]=oleft;//zapisanie probek wyjsciowych do tablicy
    	    after_right[i]=oright;
    	    aic3204_codec_write(oleft,oright);//wyslanie probki


    	    //TESTOWANIE DELTA KRONECKERA
    	     	/*for(i=0;i<SIZE;i++)
    	     	{
    	     		 left=Kronecker[i];

    	     		fir(xl,B,yl,delay,1,NH);//filtracja kanalu lewego

    	     		after_Kron[i]=oleft;

    	     	}*/


    	   // i++;
    	   // if(i==480)i=0;




//}

//FIR W C
/*    while(1){


     aic3204_codec_read(xl, xr);//pobranie probki
	 before_left[i]=left;//zapisanie probek wejsciowych do tablicy
	 before_right[i]=left;//zapisanie probek wejsciowych do tablicy
     oleft=FIR(xl,B);
     oright=FIR(xr,B);
     after_left[i]=oleft;//zapisanie probek wyjsciowych do tablicy
     after_right[i]=oleft;//zapisanie probek wyjsciowych do tablicy
     aic3204_codec_write(oleft,oright);//wyslanie probki*/

    	    //TESTOWANIE DELTA KRONECKERA
    	/*for(i=0;i<SIZE;i++)
    	{
    		 left=Kronecker[i];
    		 before_left[i]=left;//zapisanie probek wejsciowych do tablicy
    		 after_Kron[i]=FIR(xl,B);

    	}*/
    // i++;
   //  if(i==480)i=0;
   // }

    //FIR ASM




    //TESTOWANIE DELTA KRONECKERA
       	for(i=0;i<SIZE;i++)
       	{
       		 left=Kronecker[i];
       	     oleft=fir_filter(xl,B,delay);
       		 after_Kron[i]=oleft;

       	}



}












;wskazniki
;-> AR0 input
;AR1 -> wspolczynniki
;AR2 -> delay
;wartosc zwracana T0

	.def _fir_filter
_fir_filter

	;inicjalizacja rejestrow statusowych
	BSET AR3LC;
	BSET AR6LC;
	BCLR C54CM;


	;inicjalizacja adresacji kolowej
	;adresy poczatkow buforow
	MOV #AR2,BSA23; poczatek bufora opoznien
	MOV #AR1,BSA67; poczatek bufora wspolczynnikow

	;ilosc danych w buforze
	MOV  #53,BK03 ;dlugosc bufora opoznien
	MOV #51,BK47 ;dlugosc bufora wspolczynnikow

	;wskazniki na pierwszy element
	MOV #0,AR2 ;wskaznik na pierwszy element tablicy opoznien
	MOV #0,AR6 ;wskaznik na pierwszy element tablicy wspolczynnikow

	;zapisanie probki aktualnej do bufora opoznien

	MOV *AR0,*AR2+ ;

 	;filtracja
	RPT #51 ;powtorzenie 51 razy
	MAC *AR2-,*AR6+,AC0;

 	;zwrocenie wyniku
 	MOV HI(AC0),T0;

 	RET;





//filtr DP fg=2000;
const int BL = 51;
 Int16 B[51] = {
       -5,      0,      6,     16,     30,     51,     79,    117,    166,
      225,    296,    379,    471,    573,    682,    797,    913,   1030,
     1142,   1247,   1343,   1426,   1493,   1542,   1573,   1583,   1573,
     1542,   1493,   1426,   1343,   1247,   1142,   1030,    913,    797,
      682,    573,    471,    379,    296,    225,    166,    117,     79,
       51,     30,     16,      6,      0,     -5
};