/*
algorytm filtru adaptacyjnego
opartego o strukture kratowa

*/
/* ADSP-21161 System Register bit definitions */
#include "def21161.h"

/*----------------------------------------------------------------------------------*/

//parametry filtru
//poekesperymentować z innymi wartosciami
#define	rzad_fir  15 
#define krok	0.01



.section /pm	  pm_data;		//define your user program memory variables here
	
.VAR	wsp_fir[rzad_fir+1];   //deklaracja bufora na wspolczynniki fitlru
.VAR	wsp_odbicia[rzad_fir]; //deklaracja bufora na wspolczynniki odbicia

.section /dm      dm_data;		//define your user data memory variables here

.VAR	linia_wej[rzad_fir+1]; //deklaracja bufora na probki wejsciowe


.section /pm pm_code;

.EXTERN		RX_left_flag;

.EXTERN		Left_Channel_In0;
.EXTERN		Right_Channel_In0;
.EXTERN		Left_Channel_In1;
.EXTERN		Right_Channel_In1;

.EXTERN		Left_Channel_Out0;
.EXTERN		Right_Channel_Out0;

.EXTERN		Left_Channel_Out1;
.EXTERN		Right_Channel_Out1;
.EXTERN		Left_Channel_Out2;
.EXTERN		Right_Channel_Out2;

.EXTERN 	Left_Channel_AD1852;
.EXTERN 	Right_Channel_AD1852;


.GLOBAL		process_audio;
.GLOBAL		user_code_init;

	 
user_code_init:
//inicjalizacja buforow cyklicznych 
//b-adres poczatku bufora cyklicznego
//l-dlugosc bufora cyklicznego
//m-modyfikator bufora cyklicznego
//i-rejestra automatycznie inicjalizowany wraz z inicjalizacja
//rejestru b, w nim zawarty jest aktualny obliczony adres adres 

		b0=linia_wej;
		l0=@linia_wej;
		m0=0;
		m1=1;
		
		
		b8=wsp_odbicia;
		l8=@wsp_odbicia;
		b10=wsp_fir;
		l10=@wsp_fir;
		m8=0;
		m9=1;
		f7=krok;
		f0=0.0;
//zerowanie buforow kolowych
		lcntr=rzad_fir, do czysc_bufor until lce;
		  dm(i0,m1)=f0, pm(i8,m9)=f0;
czysc_bufor:	  pm(i10,m9)=f0;
		rts(db); //(db) skok opozniony wykonaja sie jeszcze dwie instrukcje
		dm(i0,m1)=f0, pm(i10,m9)=f0;
		nop;
user_code_init.end:


/* //////////////////////////////////////////////////////////////////////////////// *
 *                           audio processing routine                               *
 * //////////////////////////////////////////////////////////////////////////////// */

process_audio:
	/* loop back 'non-processed' ADC0/ADC1 data to DAC0, DAC2, and external AD1852 DAC */
	r0 = dm(Left_Channel_In0); 	dm(Left_Channel_Out1) = r0;
	r0 = dm(Right_Channel_In0); dm(Right_Channel_Out1) = r0;
	r0 = dm(Left_Channel_In1); 	dm(Left_Channel_Out2) = r0;  dm(Left_Channel_AD1852) = r0;	
	r0 = dm(Right_Channel_In1);	dm(Right_Channel_Out2) = r0; dm(Right_Channel_AD1852) = r0;	

	/* now let's process ADC1 L/R channel data and send results to DAC1 L/R channels */
	r1 = -31;        				/* scale the input samples to the range of +/-1.0 */                           
	r9 = DM(Left_Channel_In1);		/* get left AD1836 ADC0 input sample */		
	r0 = DM(Right_Channel_In1);  	/* get right AD1836 ADC0 input sample */

	dm(Left_Channel_Out2)=r9;
	dm(Right_Channel_Out2)=r0;
	
	/*
	  f9= d(n)
	  f0= x(n)
      
	*/
   	f9 = float r9 by r1; 			/* convert to floating point */
	f0 = float r0 by r1;

kratowy_lms_alg:
		f10=pass f0, f1=dm(i0,m0), f4=pm(i10,m8);
			//f0= b0(n)=x0(n), f10= f0(n), f1= b0(n-1), f4= g0(n)}
		f12=f0*f4, dm(i0,m1)=f0, f5=pm(i8,m9);
			//f12= y0(n)= b0(n)*g0(n), magazynuje b0(n), f5= k1(n)}
		f13=f1*f5, f6=f9-f12; //f13= b0(n-1)*k1(n), f6= e0(n)}
		lcntr=rzad_fir, do modyfikuj_wsp until lce;
		  f3=f0*f7, f2=f10-f13, f8=f4;
			//f2= fm(n), f3= bm-1(n)*krok, f8= gm-1(n)}
		  f13=f3*f6, f3=f10;
			//f13= krok*bm-1(n)*em-1(n), f3= fm-1(n)}
		  f13=f3*f5, f4=f8+f13, f8=f5;
			//f4= gm-1(n+1), f13= fm-1(n)*km(n), f8= km(n)}
		  f0=f1-f13, pm(i10,m9)=f4;
			//f0= bm(n), magazynuj gm-1(n+1)}
		  f10=f0*f3, f9=dm(i0,m0);
			//f10= bm(n)*fm-1(n), f9= bm(n-1)}
		  f13=f1*f2, dm(i0,m1)=f0, f4=pm(i10,m8);
			//f13= bm-1(n-1)*fm(n), magazynuje bm(n), f4= gm(n)}
		  f13=f0*f4, f1=f10+f13, f10=f6; //f10= em-1(n)}
		  	//f13= bm(n)*gm(n), f1= fm(n)*bm-1(n-1)+bm(n)*fm-1(n)}
		  f12=f12+f13, f5=pm(i8,-1);
			//f12= y(n) dla m stopnia, f5= km+1(n)}
		  f13=f1*f7, f6=f10-f13, f1=f9;
			//f6= em(n), f13= krok*f1, f1= bm(n-1)}		
		  f13=f1*f5, f8=f8+f13, f10=f2;
			//f13= bm(n-1)*km+1(n), f8= km(n+1), f10= fm(n)}
modyfikuj_wsp:     f3= f0*f6, pm(i8,2)=f8; 
			//f3= bm(n)*em(n), magazynuje km(n+1)}
		f3=f3*f7; //f3= krok*em(n)*bm(n)}
		f4=f3+f4, f3=pm(i8,-1); //f4= gm(n+1), i8 -> k1(n+1)}
		pm(i10,m9)=f4; //magazynuje gm(n+1)}
		

		/*
	  f0= bm(n)= //blad predykcji w tyl  
      f2= fm(n)= //blad predykcji w przod  
      f12= y(n)= // wyjscie filtru
      f6= em(n)= // sygnal bledu
      
		*/

	r1 = 31;						/* scale the result back up to MSBs */
	r12 = fix f12 by r1;				/* convert back to fixed point */
	r6 = fix f6 by r1;
	r0 =fix f0 by r1;
	r2 =fix f2 by r1;
	
	DM(Left_Channel_Out1) = r12; 	/* send left channel result to AD1836 Left DAC1 */
	DM(Right_Channel_Out1) = r6;
rts(db);
	DM(Left_Channel_Out0) = r12; 	/* send left channel result to AD1836 Left DAC0 */
	DM(Right_Channel_Out0) = r6;	/* send right channel result to AD1836 Right DAC0 */

process_audio.end: