/*filtr adaptacyjny oparty o nieunormowany algorytm lms
filtr fir o strukturze krawowej
jaroslaw michalski
2006_12_14
*/

#include "def21161.h"


#define	rzad_fir 5 //rozmiar rzedu filtru
#define	krok	0.01 //wartosc kroku adaptacji

.section /pm	  pm_data; 		//definicja buforow w pamieci programu
.VAR	wsp_fir[rzad_fir];      //bufor na wspolczynniki filtru 

.section /dm      dm_data;		//definicja buforow w pamieci danych
.VAR	linia_opozniajaca[rzad_fir]; //bufor na probki wejsciowe
.var 	linia_wyjsciowa[rzad_fir];   //bufor na probki wyjsciowe

.section /pm pm_code;

.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;


.GLOBAL		process_audio;  
.GLOBAL		user_code_init;
 
//tutaj inicjalizowane sa wszelakie bufory biorace udzial w przetwarzaniu

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_opozniajaca; //zawiera adres poczatku buf. cyk.
		l0=@linia_opozniajaca; //dlugosc buf. cykl.
		
		m0=-1; //modyfikator adresu w buf. cykl.
		m1=-2;
		
		b2=linia_wyjsciowa;
		l2=@linia_wyjsciowa;
		b8=wsp_fir;
		l8=@wsp_fir; 
		
		b9=b8;
		l9=l8;
		
		m8=1;
		f7=krok; //zaladowanie fo f7 stalej kroku adaptacji
		f0=0.0;  
//czyszczenie buforow, aby byc pewnym ze nie ma zadnych smieci w pamieci

		lcntr=rzad_fir, do czysc_bufor until lce;
					dm(i2,m0)=f0;
czysc_bufor:	  dm(i0,m0)=f0, pm(i8,m8)=f0; 
				
	rts; //powrot do skoku
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;  
	r0 = dm(Right_Channel_In1);	dm(Right_Channel_Out2) = r0; 	

	/* */
	        				/* scale the input samples to the range of +/-1.0 */                           
	r0 = DM(Right_Channel_In1);		/* get left AD1836 ADC0 input sample */		
	r1 = DM(Left_Channel_In1);  	/* get right AD1836 ADC0 input sample */

	DM(Left_Channel_Out2) = r1;
	/*
	 f1= d(n) 
	 f0= x(n)

	*/
	/*konwersja probek z formatu staloprzecinkowego na zmiennoprzecinkowy*/
	r2 = -31;
	f1 = float r1 by r2;//d(n) /gora bialy
   	f0 = float r0 by r2; //x(n)	/dol czerwony		/* convert to floating point */


	/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
	/* insert floating point processing algorithm here              */
	/* f0 = left channel in/out, f2 = right channel in/out          */
	/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
	
	
lms_alg:
		dm(i0,m0)=f0, f4=pm(i8,m8); 
		//magazynuje x(n) w linni opozniajacej, f4=h0(n)
		f8=f0*f4, f0=dm(i0,m0), f4=pm(i8,m8);
		//f8= x(n)*h0(n) f0= x(n-1), f4= h1(n)
		f12=f0*f4, f0=dm(i0,m0), f4= pm(i8,m8);
		//f12= x(n-1)*h1(n), f0= x(n-2), f4= h2(n)
		lcntr=rzad_fir-3, do fir until lce;
fir:		f12=f0*f4, f8=f8+f12, f0=dm(i0,m0), f4= pm(i8,m8);
		//f12= x(n-k)*hk(n), f8= sum, f0= x(n-k-1), f4= hk+1(n)
		f12=f0*f4, f8=f8+f12; //f12= x(n-N+1)*hN-1(n)
		f13=f8+f12; //f13= y(n) YYYYYYYY
		f6=f1-f13; // f6= e(n)  EEEEEEEE
		dm(i2,m0)=f13; //magazynuje w lini wyjsciowej [testy]
		f1=f6*f7, f4=dm(i0,m0); //f1= krok*e(n), f4= x(n)
		f0=f1*f4, f12=pm(i8,m8); //f0= krok*e(n)*x(n), f12= h0(n)
		lcntr=rzad_fir-1, do modyfikuj_wsp until lce;
		 	 f8=f0+f12, f4=dm(i0,m0), f12=pm(i8,m8); 
		  	//f8= hk(n+1), f4= x(n-k-1), f12= hk+1(n)
modyfikuj_wsp:   f0=f1*f4, pm(i9,m8)=f8; 
           //f0= krok*e(n)*x(n-k-1) ,zapamietanie hk(n+1)

		f8=f0+f12, f0=dm(i0,1);
		//f8= hN-1(n+1), i0 -> x(n+1) ulokowanie w linni opozniajacej
		pm(i9,m8)=f8; //zapamietanie hN-1(n+1)

/*
     
     f13= y(n)
     f6= e(n)
*/		
				

	r2 = 31;						/* scale the result back up to MSBs */
	r13 = fix f13 by r2;//y(n)				/* convert back to fixed point */
	r6 = fix f6 by r2;//e(n)

	
	DM(Left_Channel_Out0) = r13; 	/* send left channel result to AD1836 Left DAC0 */
	DM(Right_Channel_Out0) = r6;	/* send right channel result to AD1836 Right DAC0 */
rts (db);
	DM(Left_Channel_Out1) = r13; 	/* send left channel result to AD1836 Left DAC0 */
	DM(Right_Channel_Out1) = r6;	/* send right channel result to AD1836 Right DAC0 */
	
process_audio.end: