/* 
  XIAOWEN LU              SHIHJIA LEE
  In this version, we use two 2nd order filter to replace previous two 4-th order filters. 
  The coefficients of these two filters are different
  LPF and HPF   
  
  MODIFIED FROM 4.17 version
  4.24.2006
  Combine square and accumulation in the filter routine. result is 16bit. 
  4.28 add one more filter from 200-350, 350-500Hz
  4.29 Modify filters from 0-50, 50-350,350-500,500-750,750-1k,1k-1.5k,>1.5k, total is seven filters.
  Use correlation to calculate the difference of the fingerprint instead of absolution value
  4.30 In order to avoid small correlation result, we add Euclidian distance when Correlation doesnt' work

*/

#include <Mega32.h> 
#include <delay.h> 
#include <stdio.h> 
#include <stdlib.h>   
#include <math.h>                   
//#include <debugger.c>

#asm
    .equ __lcd_port=0x15
#endasm
#include <lcd.h> // LCD driver routines
#include <delay.h> // delay_ms
#define LCDwidth 16 //characters
      
#define t1 250        
#define begin {
#define end }

#define IDLE 0
#define STORE 1
#define TEST 2
#define COMPARE 3  
#define MY_DEBUG 4
#define MY_DEBUG2 5
#define RECORD_DIC 6
#define RECORD_WORD 7   

//#define CORRELATION
#define EUCLID

#define num_dic 8
#define size_dic 40
char lcd_buffer[17];    // LCD display buffer 

#define float2fix(a) ((int)((a)*256.0)) 
#define int2fix(a)   (((int)(a))<<8)            //Convert char to fix  
#define fix2float4_12(a) ((float)(a)/4096.0)   
#define fix2float(a) ((float)(a)/256.0)         //Convert fix to float  

//flash

unsigned char state=IDLE;
unsigned int k=0,m=0,Ain;
unsigned char counter=t1, code_id=0;    
unsigned char store_flag,test_flag;

int p1=0,pfir,paf=0,p2=0,p3=0,p4=0,p5=0;
int dic[num_dic][size_dic],word[size_dic];   


//the following variables are stored in the RAM
#pragma regalloc-
 //0-50
 int b1_1,b1_2,b1_3,a1_2,a1_3,  x1n, x1n_1, x1n_2, y1n, y1n_1, y1n_2;  
                                          
 //50-350
 int b1_1fir,b2_1fir,b3_1fir,a2_1fir,a3_1fir,b1_2fir,b2_2fir,b3_2fir,a2_2fir,a3_2fir,   
     xnfir, xn_1fir, xn_2fir, yn_1fir, yn_2fir,      ynfir,yn_1_2fir, yn_2_2fir, zn_1fir, zn_2fir, znfir;   
     
 //350-500
 int b1_1af,b2_1af,b3_1af,a2_1af,a3_1af,b1_2af,b2_2af,b3_2af,a2_2af,a3_2af,   
     xnaf, xn_1af, xn_2af, yn_1af, yn_2af,      ynaf,yn_1_2af, yn_2_2af, zn_1af, zn_2af, znaf;    
 
//500-750
 int b1_1s,b2_1s,b3_1s,a2_1s,a3_1s,b1_2s,b2_2s,b3_2s,a2_2s,a3_2s,   
     xns, xn_1s, xn_2s, yn_1s, yn_2s,      yns,yn_1_2s, yn_2_2s, zn_1s, zn_2s, zns;

 //750-1k
 int b1_1t,b2_1t,b3_1t,a2_1t,a3_1t,b1_2t,b2_2t,b3_2t,a2_2t,a3_2t,
     xnt, xn_1t, xn_2t, yn_1t, yn_2t,      ynt,yn_1_2t, yn_2_2t, zn_1t, zn_2t, znt;
//1k-1.5k
int  b1_1f,b2_1f,b3_1f,a2_1f,a3_1f,b1_2f,b2_2f,b3_2f,a2_2f,a3_2f,
     xnf, xn_1f, xn_2f, yn_1f, yn_2f,      ynf,yn_1_2f, yn_2_2f, zn_1f, zn_2f, znf;              
//>1.5k
int   b5_1,b5_2,b5_3,a5_2,a5_3, x5n, x5n_1, x5n_2, y5n, y5n_1, y5n_2;  
 
 int gfir,gaf,g2,g3, g4;
 
#pragma regalloc+


int IIR2_1(int xx,int c);    
int IIR4_fir(int xx, int c); //50-350     
int IIR4_af(int xx, int c);  //350-500
int IIR4_s(int xx, int c);   //500-750  
int IIR4_t(int xx, int c);   //750-1000  
int IIR4_f(int xx, int c);   //1000-1500  
int IIR2_5(int xx, int c);   //>1500  

void analysis(int xx); 
unsigned char lookup();
unsigned char find_min(float a[num_dic]);
void get_average();
float calcorrelation(int a[num_dic], int b[num_dic]);

interrupt [TIM0_COMP] void timer0_compare(void)    
{                                         
    
    if(state == STORE || state==TEST)
    {   

        Ain=ADCH;    
        ADCSR.6=1; 
        
        if(Ain>150) 
        {   

          if (state==STORE) { state= RECORD_DIC;   store_flag = 0;}
          else if(state==TEST) { state=RECORD_WORD;   test_flag=0;}
          else {} 
         
        }
         
    } 
    else if ((state==RECORD_DIC || state==RECORD_WORD) && counter>0)
    {     
       counter--;         
       
       analysis(Ain);
       Ain = ADCH;
       ADCSR.6=1;
    }
       
    else if(counter==0)
    {    

         counter = t1;
         if(state==RECORD_WORD)
         {
//            word[m++]=p1;
            word[m++]=pfir;////50-350   
            word[m++]=paf;  
            word[m++]=p2;
            word[m++]=p3; 
            word[m++]=p4; 
    //        word[m++]=p5; 
            
            
            if(m>=size_dic) 
            {
              state=COMPARE;
              test_flag=0;
              m=0;           
           
              p1 = 0;
              pfir =0; 
              paf=0;
              p2 = 0;
              p3 = 0;
              p4 = 0;
 //             p5 = 0;  

//              printf("WORD IS DONE!\n");      

            }
         }
         else if(state==RECORD_DIC)
         {                                        
//            dic[code_id][k++]=p1;////<50Hz p1; 
            dic[code_id][k++]=pfir;////50-350 pfir; 
            dic[code_id][k++]=paf;  
            dic[code_id][k++]=p2;
            dic[code_id][k++]=p3; 
            dic[code_id][k++]=p4;  
   //         dic[code_id][k++]=p5; 
            
            
            if(k>=size_dic)
            {            
               p1 = 0;          
               pfir =0;
               paf=0;
               p2 = 0;
               p3 = 0;
               p4 = 0;
               p5 = 0;                    
               store_flag = 0;
               k=0;
               state = IDLE;                         
               printf("one code was generated!\r\n");
            
               code_id++;
               if(code_id==num_dic)
               {
                    printf("DICTIONARY IS FULL!\n");    
               }             
            }
        }                          
  }//end of if_counter==0
} 
                                    

void main(void)
{    
   unsigned char index;       
   int i,j;
   test_flag=1;
   store_flag=1;
 
   ADMUX = 0b00100000;   
   ADCSR = 0b11000101; //prescalar is 32
 
   DDRC = 0xFF;                                                        
   DDRA = 0x00;    //INPUT from ADC

   TIMSK=0b00000010;  //turn on timer 0 cmp match ISR 
   OCR0 = 63;   
   //prescalar to 8 and turn on clear-on-match
   TCCR0=0b00001011;
   k=0; 
   m=0;  
 
   UCSRB = 0x18 ;
   UBRRL = 103 ;
   putsf("\r\nStarting...\r\n");                                                 
   printf("The maximum number of code is %d\n\r", num_dic);

   lcd_init(LCDwidth);       //initialize the display
   lcd_clear();         //clear the display
  
   lcd_gotoxy(0,0);      //position to upper left on display
   lcd_putsf("WHAT YOU SAID IS:");   //another string from flash

   
/* the following are the coeffients for FIVE filters */
/* LPF: cutoff 50Hz */
    b1_1 = float2fix(0.09796896856234) ;
    b1_2 = float2fix(-0.19473177368601   ) ;
    b1_3 = float2fix(0.09796896856234) ;                   
    a1_2 = float2fix(1.95274547233410) ;   //note that sign is negated from design input
    a1_3 = float2fix( -0.95395163577278) ;     
    
   /* the following is for first bandpass filter */
/* BPF 50-350Hz */  
       
    b1_1fir = float2fix(.0358505978767782) ;
    b2_1fir = float2fix(-0.0541871288157825        ) ;
    b3_1fir = float2fix(0.0358505978767782 );             
    a2_1fir = float2fix(1.73538719156564);   //note that sign is negated from design input
    a3_1fir = float2fix(-0.821982126182528) ;    
     /* the following is for TWO filter */
         
    b1_2fir = float2fix(52.1567830371572) ;
    b2_2fir = float2fix(-104.129598795861          ) ;
    b3_2fir = float2fix(52.1567830371574) ;               
    a2_2fir = float2fix(1.89406507833894         ) ;   //note that sign is negated from design input
    a3_2fir = float2fix(-0.913269977916045 ) ;   
    
    
    /* BPF 350-500Hz */                        
    b1_1af = float2fix(0.278956417790016) ;
    b2_1af = float2fix(-0.371529668270894) ;
    b3_1af = float2fix(0.278956417790016);                
    a2_1af = float2fix(1.47601438972325);   //note that sign is negated from design input
    a3_1af = float2fix(-0.928175676794432) ;    
     /* the following is for TWO filter */                          
     
    b1_2af = float2fix(7.43116419052062 ) ;
    b2_2af = float2fix(-12.9683446950251) ;
    b3_2af = float2fix(7.43116419052061) ;                
    a2_2af = float2fix(1.57433029063583) ;   //note that sign is negated from design input
    a3_2af = float2fix(-0.934881249869463) ; 
    
/* the following is for TWO filter */
/* BPF 500-750Hz */             
    b1_1s = float2fix(0.29045042833981) ;
    b2_1s = float2fix(-0.173064360770252) ;
    b3_1s = float2fix(0.290450428339811);                
    a2_1s = float2fix(0.95949121766971);   //note that sign is negated from design input
    a3_1s = float2fix(-0.8830505407161) ;    
     /* the following is for TWO filter */                         
    b1_2s = float2fix(3.27756375456152) ;
    b2_2s = float2fix(-4.9288608086426) ;
    b3_2s = float2fix(3.27756375456153) ;                
    a2_2s = float2fix(1.1704755856173) ;   //note that sign is negated from design input
    a3_2s = float2fix(-0.89264687697597) ;
    
/* the following for THREE filter */
/* BPF 750-1000Hz*/                     
    b1_1t = float2fix(0.362073778695786 ) ;
    b2_1t = float2fix(0.0589871600918791) ;
    b3_1t = float2fix(0.362073778695785) ;               
    a2_1t = float2fix(0.251665993296962 ) ;   //note that sign is negated from design input
    a3_1t = float2fix(-0.886419800815432) ;
   /* the following for THREE filter */     
                     
    b1_2t = float2fix(2.34874546847157) ;
    b2_2t = float2fix(-2.1143867571147) ;
    b3_2t = float2fix(2.34874546847158 ) ;             
    a2_2t = float2fix(0.496278883588266) ;   //note that sign is negated from design input
    a3_2t = float2fix(-0.889253947911637) ;     

/* BPF 1.0k-1.5kHz */
    b1_1f = float2fix(0.273537710760694) ;
    b2_1f = float2fix(-0.0895416442264191         ) ;
    b3_1f = float2fix(0.273537710760694) ;           
    a2_1f = float2fix(-0.497835016967794) ;   //note that sign is negated from design input
    a3_1f = float2fix(-0.770655067394278) ; 


    b1_2f = float2fix(1.74027097753613) ;
    b2_2f = float2fix(2.71640486024015) ;
    b3_2f = float2fix(1.74027097753613) ;              
    a2_2f = float2fix(-0.954207140409156) ;   //note that sign is negated from design input
    a3_2f = float2fix(-0.794824195146064           ) ; 
    

/* HPF cutoff frequency: 1.5k Hz */             
    b5_1 = float2fix(0.104700735128835) ;
    b5_2 = float2fix(0.102406046057712) ;
    b5_3 = float2fix(0.104700735128835) ;                    
    a5_2 = float2fix(-1.50553986947694) ;   //note that sign is negated from design input
    a5_3 = float2fix(-0.612535293676895     ) ;      
    
    gfir=float2fix(0.0516151198830175);//50-350
    gaf=float2fix(0.0461824650452957); //350-500
    g2=float2fix(0.100560438947369); //500-750
    g3=float2fix(0.112568609179043); //750-1000   
    g4=float2fix(0.219945785914816); //1000-1500     
    
    for (j=0;j<num_dic;j++)
    {
      for (i =0; i<size_dic; i++)
      {
        dic[j][i] = 0;
      }
    }
    m=0;
   #asm
    sei
   #endasm   
   
    //endless loop to read keyboard
    while(1) 
    { 
 
     if(PINB.0==0 && store_flag==1 )//CREATE THE CODEBOOK
     {
//            PORTC.0=PINB.0; 
            state = STORE;  
//            flag = 1;
     }
     else if(PINB.0==1 && store_flag==0)//RELEASE BUTTION0 
     {   
         
          state = IDLE;
          store_flag=1;     
     }
     else if(PINB.1==0 && test_flag ==1)//SPEACH a testing word
     {              
           state = TEST;  
          
//           test_flag =0;  
     }                                                     
     else if(PINB.1==1 && test_flag==0)
     {
//        state = IDLE;

          test_flag=1;  
//          printf("state:%d\t flag: %d\r\n", state,test_flag);                
     }
     else if(state==COMPARE)
     {     
           index = lookup(); 
          
           state = IDLE; 
//           test_flag = 0;

           if(index==1)
           {
              printf("ONE \n");
              lcd_gotoxy(5,1);      //position to upper left on display  
              lcd_putsf("HELLO        "); 
           }   
           else if(index==2)
           {             
              printf("TWO \n");
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("LEFT        "); 
           }
           else if(index==3)
           {
              printf(" THREE \n");
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("RIGHT        ") ;
                //LED2
           }    
           
             else if(index==4)
           {
              printf(" FOUR \n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
              lcd_putsf("BYE        "); 
           }                            
               
             else if(index==5)
           {
              printf(" FIVE \n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("CLOSE        ");
           }   
              
               else if(index==6)
           {
              printf(" SIX \n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("SIX        ");
           }  
           
               else if(index==7)
           {
              printf(" SEVEN \n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("SEVEN        ");
           }  
           
               else if(index==8)
           {
              printf(" EIGHT \n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("EIGHT        ");
           }  
           
           else
           {
              printf("NO MATCH\n"); 
              lcd_gotoxy(5,1);      //position to upper left on display  
                lcd_putsf("NO MATCH        ") ;
           }                                    
     }               
     else if (PINB.7==0 ) 
     {   
         for(j=0;j<code_id;j++)
         {   
            for(i=0;i<size_dic;i++)
            {               
               printf("%f \r\n", fix2float(dic[j][i]));  
            }
//            if(k>=size_dic)  scanf("%f",fIn);   
            printf("Next Code:\n\r");
             
          } 
          for(i=0;i<size_dic;i++)
             printf("%f \r\n", fix2float(word[i]));  
          printf("one\r\n\n\n");
          
          get_average();
          code_id = 0;            
          state=IDLE;  
     }
  

     else
     {     
           if(state==COMPARE){}                    
     }                 
   } // end while    

}  
         
 #pragma warn-
/*This is fixed point 2nd IIR filter */   
// 0-50Hz  
int IIR2_1(int xx, int c)
{
    #asm
    .macro mult_acc         ;r31:r30:r24 += r23:r22 * r21:r20
     muls r23, r21  ; (signed)ah * (signed)bh
     add r31, r0
     mul r22, r20  ; al * bl
     add r24, r0
     adc r30, r1
     adc r31, r27
     mulsu   r23, r20        ; (signed)ah * bl
     add r30, r0
     adc r31, r1
     mulsu r21, r22 ; (signed)bh * al
     add r30, r0
     adc r31, r1
     .endm
    
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31 

    
    lds  R22, _b1_1       ;load b1 from RAM
    lds  R23, _b1_1+1 
    ldd   R20, Y+2         ;load input parameter xx from stack
    ldd  R21, Y+3  
    mult_acc            ; b1*xx  
    

    
    lds  R22, _b1_2       ;load b2 from RAM
    lds  R23, _b1_2+1 
    lds  R20, _x1n_1     ;load x(n-1) from  RAM
    lds  R21, _x1n_1+1
    mult_acc            ; b2*x(n-1) 
    
    lds  R22, _b1_3       ;load b3 from RAM
    lds  R23, _b1_3+1 
    lds  R20, _x1n_2     ;load x(n-2) from  RAM
    lds  R21, _x1n_2+1
    mult_acc            ; b3*x(n-2)
    
    lds  R22, _a1_2       ;load -a2 from RAM
    lds  R23, _a1_2+1 
    lds  R20, _y1n_1     ;load y(n-1) from  RAM
    lds  R21, _y1n_1+1
    mult_acc            ; -a2*y(n-1)  
    
    lds  R22, _a1_3       ;load -a3 from RAM
    lds  R23, _a1_3+1 
    lds  R20, _y1n_2     ;load y(n-2) from  RAM
    lds  R21, _y1n_2+1
    mult_acc            ; -a3*y(n-2)
    
    lds  R20, _x1n_1     ;load x(n-1) from  RAM
    lds  R21, _x1n_1+1
    sts  _x1n_2, r20      ;store x(n-2) to  RAM
    sts  _x1n_2+1, R21 
    ldd   R20, Y+2         ;load input parameter xx from stack
    ldd  R21, Y+3
    sts  _x1n_1, r20     ;store x(n-1) to  RAM
    sts  _x1n_1+1, R21 
    lds  R20, _y1n_1     ;load y(n-1) from  RAM
    lds  R21, _y1n_1+1
    sts  _y1n_2, R20     ;store y(n-2) to  RAM
    sts  _y1n_2+1, R21 
    sts  _y1n_1, r30     ;store new output as y(n-1) to  RAM
    sts  _y1n_1+1, r31 
   
	
	mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
 
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }       
/* 50-350Hz*/
int IIR4_fir(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
	  .macro mult_acc         ;r31:r30:r24 += r23:r22 * r21:r20
     muls r23, r21  ; (signed)ah * (signed)bh
     add r31, r0
     mul r22, r20  ; al * bl
     add r24, r0
     adc r30, r1
     adc r31, r27
     mulsu   r23, r20        ; (signed)ah * bl
     add r30, r0
     adc r31, r1
     mulsu r21, r22 ; (signed)bh * al
     add r30, r0
     adc r31, r1
     .endm

	
    .macro mults	;r31:r30:rxx = r23:r22 * r21:r20
	muls	r23, r21		; (signed)ah * (signed)bh
	mov		r31, r0         ;r18, r0
	mul		r22, r20		; al * bl
	mov     r30, r1         ;movw	r17:r16, r1:r0
	;mov     r16, r0
	mulsu	r23, r20		; (signed)ah * bl
	add		r30, r0         ;r17, r0
	adc		r31, r1         ;r18, r1
	mulsu	r21, r22		; (signed)bh * al
	add		r30, r0         ;r17, r0
	adc		r31, r1         ;r18, r1
	.endm
            
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    ;*********first filter*****************input is x output is y********************    

    lds  R22, _b1_1fir       ;load b1-first from RAM
    lds  R23, _b1_1fir+1 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    mult_acc              ; b1*x  
    
    lds  R22, _b2_1fir       ;load b2-first from RAM
    lds  R23, _b2_1fir+1 
    lds  R20, _xn_1fir     ;load x(n-1) from  RAM
    lds  R21, _xn_1fir+1
    mult_acc              ; b2*x(n-1) 
    
    lds  R22, _b3_1fir       ;load b3-first from RAM
    lds  R23, _b3_1fir+1 
    lds  R20, _xn_2fir     ;load x(n-2) from  RAM
    lds  R21, _xn_2fir+1
    mult_acc              ; b3*x(n-2)
    
    lds  R22, _a2_1fir       ;load -a2 - first from RAM
    lds  R23, _a2_1fir+1 
    lds  R20, _yn_1fir     ;load y(n-1) from  RAM
    lds  R21, _yn_1fir+1
    mult_acc              ; -a2*y(n-1)  
    
    lds  R22, _a3_1fir       ;load -a3 - first from RAM
    lds  R23, _a3_1fir+1 
    lds  R20, _yn_2fir     ;load y(n-2) from  RAM
    lds  R21, _yn_2fir+1
    mult_acc              ; -a3*y(n-2)      
    
    lds  R20, _xn_1fir       ;load x(n-1) -first from  RAM
    lds  R21, _xn_1fir+1
    sts  _xn_2fir, r20      ;store x(n-2) to  RAM
    sts  _xn_2fir+1, R21 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    sts  _xn_1fir, r20     ;store x(n-1) to  RAM
    sts  _xn_1fir+1, R21 
    lds  R20, _yn_1fir     ;load y(n-1) from  RAM
    lds  R21, _yn_1fir+1
    sts  _yn_2fir, R20     ;store y(n-2) to  RAM
    sts  _yn_2fir+1, R21 
    sts  _yn_1fir, r30     ;store new output as y(n-1) to  RAM
    sts  _yn_1fir+1, r31

    ;*******second filter**************** input is y output is z**********************

    lds  R22, _b1_2fir       ;load b1-second from RAM
    lds  R23, _b1_2fir+1 
     mov  R20, r30        ;load output from first filter (y from r30-r31)
     mov  R21, r31

     sts  _ynfir, R30
     sts  _ynfir+1, R31    
     
     ;lds R20, _yn
    ; lds R21, _yn+1

     clr r27
     clr r24
     clr r30
     clr r31
     mult_acc             ; b1*y  
    
    lds  R22, _b2_2fir       ;load b2-second from RAM
    lds  R23, _b2_2fir+1 
    lds  R20, _yn_1_2fir       ;load y(n-1) from  RAM
    lds  R21, _yn_1_2fir+1
    mult_acc              ; b2*y(n-1) 
    
    lds  R22, _b3_2fir       ;load b3-second from RAM
    lds  R23, _b3_2fir+1 
    lds  R20, _yn_2_2fir       ;load y(n-2) from  RAM
    lds  R21, _yn_2_2fir+1
    mult_acc              ; b3*y(n-2)
    
    lds  R22, _a2_2fir       ;load -a2-second from RAM
    lds  R23, _a2_2fir+1 
    lds  R20, _zn_1fir       ;load z(n-1) from  RAM
    lds  R21, _zn_1fir+1
    mult_acc              ; -a2*z(n-1)  
     
    lds  R22, _a3_2fir       ;load -a3-second from RAM
    lds  R23, _a3_2fir+1 
    lds  R20, _zn_2fir     ;load z(n-2) from  RAM
    lds  R21, _zn_2fir+1
    mult_acc              ; -a3*z(n-2)     
    
    sts _znfir, R30
    sts _znfir+1,R31
    
    lds R22, _gfir
    lds R23, _gfir+1   
    mov R20, R30
    mov R21, R31
    mults       ; output *gfir
    

    lds  R20, _yn_1_2fir      ;load y(n-1) from  RAM
    lds  R21, _yn_1_2fir+1
    sts  _yn_2_2fir, r20      ;store y(n-2) to  RAM
    sts  _yn_2_2fir+1, R21 
    lds   R20, _ynfir            ;load input parameter y(n) from stack
    lds  R21, _ynfir+1                                  
    
    sts  _yn_1_2fir, r20      ;store y(n-1) to  RAM
    sts  _yn_1_2fir+1, R21 
    lds  R20, _zn_1fir      ;load z(n-1) from  RAM
    lds  R21, _zn_1fir+1
    sts  _zn_2fir, R20      ;store z(n-2) to  RAM
    sts  _zn_2fir+1, R21  
    
    lds R20, _znfir
    lds R21, _znfir+1
    sts  _zn_1fir, r20      ;store new output as z(n-1) to  RAM
    sts  _zn_1fir+1, r21
   
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }      
  
 
 // 350-500Hz
 int IIR4_af(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
	
            
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    ;*********afst filter*****************input is x output is y********************    

    lds  R22, _b1_1af       ;load b1-afst from RAM
    lds  R23, _b1_1af+1 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    mult_acc              ; b1*x  
    
    lds  R22, _b2_1af       ;load b2-afst from RAM
    lds  R23, _b2_1af+1 
    lds  R20, _xn_1af     ;load x(n-1) from  RAM
    lds  R21, _xn_1af+1
    mult_acc              ; b2*x(n-1) 
    
    lds  R22, _b3_1af       ;load b3-afst from RAM
    lds  R23, _b3_1af+1 
    lds  R20, _xn_2af     ;load x(n-2) from  RAM
    lds  R21, _xn_2af+1
    mult_acc              ; b3*x(n-2)
    
    lds  R22, _a2_1af       ;load -a2 - afst from RAM
    lds  R23, _a2_1af+1 
    lds  R20, _yn_1af     ;load y(n-1) from  RAM
    lds  R21, _yn_1af+1
    mult_acc              ; -a2*y(n-1)  
    
    lds  R22, _a3_1af       ;load -a3 - afst from RAM
    lds  R23, _a3_1af+1 
    lds  R20, _yn_2af     ;load y(n-2) from  RAM
    lds  R21, _yn_2af+1
    mult_acc              ; -a3*y(n-2)      
    
    lds  R20, _xn_1af       ;load x(n-1) -afst from  RAM
    lds  R21, _xn_1af+1
    sts  _xn_2af, r20      ;store x(n-2) to  RAM
    sts  _xn_2af+1, R21 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    sts  _xn_1af, r20     ;store x(n-1) to  RAM
    sts  _xn_1af+1, R21 
    lds  R20, _yn_1af     ;load y(n-1) from  RAM
    lds  R21, _yn_1af+1
    sts  _yn_2af, R20     ;store y(n-2) to  RAM
    sts  _yn_2af+1, R21 
    sts  _yn_1af, r30     ;store new output as y(n-1) to  RAM
    sts  _yn_1af+1, r31

    ;*******second filter**************** input is y output is z**********************

    lds  R22, _b1_2af       ;load b1-second from RAM
    lds  R23, _b1_2af+1 
     mov  R20, r30        ;load output from afst filter (y from r30-r31)
     mov  R21, r31

     sts  _ynaf, R30
     sts  _ynaf+1, R31    
     
     ;lds R20, _yn
    ; lds R21, _yn+1

     clr r27
     clr r24
     clr r30
     clr r31
     mult_acc             ; b1*y  
    
    lds  R22, _b2_2af       ;load b2-second from RAM
    lds  R23, _b2_2af+1 
    lds  R20, _yn_1_2af       ;load y(n-1) from  RAM
    lds  R21, _yn_1_2af+1
    mult_acc              ; b2*y(n-1) 
    
    lds  R22, _b3_2af       ;load b3-second from RAM
    lds  R23, _b3_2af+1 
    lds  R20, _yn_2_2af       ;load y(n-2) from  RAM
    lds  R21, _yn_2_2af+1
    mult_acc              ; b3*y(n-2)
    
    lds  R22, _a2_2af       ;load -a2-second from RAM
    lds  R23, _a2_2af+1 
    lds  R20, _zn_1af       ;load z(n-1) from  RAM
    lds  R21, _zn_1af+1
    mult_acc              ; -a2*z(n-1)  
     
    lds  R22, _a3_2af       ;load -a3-second from RAM
    lds  R23, _a3_2af+1 
    lds  R20, _zn_2af     ;load z(n-2) from  RAM
    lds  R21, _zn_2af+1
    mult_acc              ; -a3*z(n-2)     
    
    sts _znaf, R30
    sts _znaf+1,R31
    
    lds R22, _gaf
    lds R23, _gaf+1   
    mov R20, R30
    mov R21, R31
    mults       ; output *gaf
    

    lds  R20, _yn_1_2af      ;load y(n-1) from  RAM
    lds  R21, _yn_1_2af+1
    sts  _yn_2_2af, r20      ;store y(n-2) to  RAM
    sts  _yn_2_2af+1, R21 
    lds   R20, _ynaf            ;load input parameter y(n) from stack
    lds  R21, _ynaf+1                                  
    
    sts  _yn_1_2af, r20      ;store y(n-1) to  RAM
    sts  _yn_1_2af+1, R21 
    lds  R20, _zn_1af      ;load z(n-1) from  RAM
    lds  R21, _zn_1af+1
    sts  _zn_2af, R20      ;store z(n-2) to  RAM
    sts  _zn_2af+1, R21  
    
    lds R20, _znaf
    lds R21, _znaf+1
    sts  _zn_1af, r20      ;store new output as z(n-1) to  RAM
    sts  _zn_1af+1, r21
   
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }      

 

//500-750Hz
int IIR4_s(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
	
            
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    ;*********first filter*****************input is x output is y********************    

    lds  R22, _b1_1s       ;load b1-first from RAM
    lds  R23, _b1_1s+1 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    mult_acc              ; b1*x  
    
    lds  R22, _b2_1s       ;load b2-first from RAM
    lds  R23, _b2_1s+1 
    lds  R20, _xn_1s     ;load x(n-1) from  RAM
    lds  R21, _xn_1s+1
    mult_acc              ; b2*x(n-1) 
    
    lds  R22, _b3_1s       ;load b3-first from RAM
    lds  R23, _b3_1s+1 
    lds  R20, _xn_2s     ;load x(n-2) from  RAM
    lds  R21, _xn_2s+1
    mult_acc              ; b3*x(n-2)
    
    lds  R22, _a2_1s       ;load -a2 - first from RAM
    lds  R23, _a2_1s+1 
    lds  R20, _yn_1s     ;load y(n-1) from  RAM
    lds  R21, _yn_1s+1
    mult_acc              ; -a2*y(n-1)  
    
    lds  R22, _a3_1s       ;load -a3 - first from RAM
    lds  R23, _a3_1s+1 
    lds  R20, _yn_2s     ;load y(n-2) from  RAM
    lds  R21, _yn_2s+1
    mult_acc              ; -a3*y(n-2)      
    
    lds  R20, _xn_1s       ;load x(n-1) -first from  RAM
    lds  R21, _xn_1s+1
    sts  _xn_2s, r20      ;store x(n-2) to  RAM
    sts  _xn_2s+1, R21 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    sts  _xn_1s, r20     ;store x(n-1) to  RAM
    sts  _xn_1s+1, R21 
    lds  R20, _yn_1s     ;load y(n-1) from  RAM
    lds  R21, _yn_1s+1
    sts  _yn_2s, R20     ;store y(n-2) to  RAM
    sts  _yn_2s+1, R21 
    sts  _yn_1s, r30     ;store new output as y(n-1) to  RAM
    sts  _yn_1s+1, r31

    ;*******second filter**************** input is y output is z**********************

    lds  R22, _b1_2s       ;load b1-second from RAM
    lds  R23, _b1_2s+1 
     mov  R20, r30        ;load output from first filter (y from r30-r31)
     mov  R21, r31

     sts  _yns, R30
     sts  _yns+1, R31    
     
     ;lds R20, _yn
    ; lds R21, _yn+1

     clr r27
     clr r24
     clr r30
     clr r31
     mult_acc             ; b1*y  
    
    lds  R22, _b2_2s       ;load b2-second from RAM
    lds  R23, _b2_2s+1 
    lds  R20, _yn_1_2s       ;load y(n-1) from  RAM
    lds  R21, _yn_1_2s+1
    mult_acc              ; b2*y(n-1) 
    
    lds  R22, _b3_2s       ;load b3-second from RAM
    lds  R23, _b3_2s+1 
    lds  R20, _yn_2_2s       ;load y(n-2) from  RAM
    lds  R21, _yn_2_2s+1
    mult_acc              ; b3*y(n-2)
    
    lds  R22, _a2_2s       ;load -a2-second from RAM
    lds  R23, _a2_2s+1 
    lds  R20, _zn_1s       ;load z(n-1) from  RAM
    lds  R21, _zn_1s+1
    mult_acc              ; -a2*z(n-1)  
     
    lds  R22, _a3_2s       ;load -a3-second from RAM
    lds  R23, _a3_2s+1 
    lds  R20, _zn_2s     ;load z(n-2) from  RAM
    lds  R21, _zn_2s+1
    mult_acc              ; -a3*z(n-2)     
    
    sts _zns, R30
    sts _zns+1,R31
    
    lds R22, _g2
    lds R23, _g2+1   
    mov R20, R30
    mov R21, R31
    mults       ; output *g2
    

    lds  R20, _yn_1_2s      ;load y(n-1) from  RAM
    lds  R21, _yn_1_2s+1
    sts  _yn_2_2s, r20      ;store y(n-2) to  RAM
    sts  _yn_2_2s+1, R21 
    lds   R20, _yns            ;load input parameter y(n) from stack
    lds  R21, _yns+1                                  
    
    sts  _yn_1_2s, r20      ;store y(n-1) to  RAM
    sts  _yn_1_2s+1, R21 
    lds  R20, _zn_1s      ;load z(n-1) from  RAM
    lds  R21, _zn_1s+1
    sts  _zn_2s, R20      ;store z(n-2) to  RAM
    sts  _zn_2s+1, R21  
    
    lds R20, _zns
    lds R21, _zns+1
    sts  _zn_1s, r20      ;store new output as z(n-1) to  RAM
    sts  _zn_1s+1, r21
   
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }      
 //750-1000Hz
 int IIR4_t(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
   
    
                
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    ;*********first filter*****************input is x output is y********************    

    lds  R22, _b1_1t       ;load b1-first from RAM
    lds  R23, _b1_1t+1 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    mult_acc              ; b1*x    
    
    
    lds  R22, _b2_1t       ;load b2-first from RAM
    lds  R23, _b2_1t+1 
    lds  R20, _xn_1t     ;load x(n-1) from  RAM
    lds  R21, _xn_1t+1
    mult_acc              ; b2*x(n-1) 
    
    lds  R22, _b3_1t       ;load b3-first from RAM
    lds  R23, _b3_1t+1 
    lds  R20, _xn_2t     ;load x(n-2) from  RAM
    lds  R21, _xn_2t+1
    mult_acc              ; b3*x(n-2)
    
    lds  R22, _a2_1t       ;load -a2 - first from RAM
    lds  R23, _a2_1t+1 
    lds  R20, _yn_1t     ;load y(n-1) from  RAM
    lds  R21, _yn_1t+1
    mult_acc              ; -a2*y(n-1)  
    
    lds  R22, _a3_1t       ;load -a3 - first from RAM
    lds  R23, _a3_1t+1 
    lds  R20, _yn_2t     ;load y(n-2) from  RAM
    lds  R21, _yn_2t+1
    mult_acc              ; -a3*y(n-2)      
    
    lds  R20, _xn_1t       ;load x(n-1) -first from  RAM
    lds  R21, _xn_1t+1
    sts  _xn_2t, r20      ;store x(n-2) to  RAM
    sts  _xn_2t+1, R21 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    sts  _xn_1t, r20     ;store x(n-1) to  RAM
    sts  _xn_1t+1, R21 
    lds  R20, _yn_1t     ;load y(n-1) from  RAM
    lds  R21, _yn_1t+1
    sts  _yn_2t, R20     ;store y(n-2) to  RAM
    sts  _yn_2t+1, R21 
    sts  _yn_1t, r30     ;store new output as y(n-1) to  RAM
    sts  _yn_1t+1, r31

    ;*******second filter**************** input is y output is z**********************

    lds  R22, _b1_2t       ;load b1-second from RAM
    lds  R23, _b1_2t+1 
     mov  R20, r30        ;load output from first filter (y from r30-r31)
     mov  R21, r31

     sts  _ynt, R30
     sts  _ynt+1, R31    
     
     ;lds R20, _yn
    ; lds R21, _yn+1

     clr r27
     clr r24
     clr r30
     clr r31
     mult_acc             ; b1*y  
    
    lds  R22, _b2_2t       ;load b2-second from RAM
    lds  R23, _b2_2t+1 
    lds  R20, _yn_1_2t       ;load y(n-1) from  RAM
    lds  R21, _yn_1_2t+1
    mult_acc              ; b2*y(n-1) 
    
    lds  R22, _b3_2t       ;load b3-second from RAM
    lds  R23, _b3_2t+1 
    lds  R20, _yn_2_2t       ;load y(n-2) from  RAM
    lds  R21, _yn_2_2t+1
    mult_acc              ; b3*y(n-2)
    
    lds  R22, _a2_2t       ;load -a2-second from RAM
    lds  R23, _a2_2t+1 
    lds  R20, _zn_1t       ;load z(n-1) from  RAM
    lds  R21, _zn_1t+1
    mult_acc              ; -a2*z(n-1)  
     
    lds  R22, _a3_2t       ;load -a3-second from RAM
    lds  R23, _a3_2t+1 
    lds  R20, _zn_2t     ;load z(n-2) from  RAM
    lds  R21, _zn_2t+1
    mult_acc              ; -a3*z(n-2)     
    
    sts _znt, R30
    sts _znt+1,R31
    
    lds R22, _g3
    lds R23, _g3+1   
    mov R20, R30
    mov R21, R31
    mults       ; output *g2
    

    lds  R20, _yn_1_2t      ;load y(n-1) from  RAM
    lds  R21, _yn_1_2t+1
    sts  _yn_2_2t, r20      ;store y(n-2) to  RAM
    sts  _yn_2_2t+1, R21 
    lds   R20, _ynt            ;load input parameter y(n) from stack
    lds  R21, _ynt+1                                  
    
    sts  _yn_1_2t, r20      ;store y(n-1) to  RAM
    sts  _yn_1_2t+1, R21 
    lds  R20, _zn_1t      ;load z(n-1) from  RAM
    lds  R21, _zn_1t+1
    sts  _zn_2t, R20      ;store z(n-2) to  RAM
    sts  _zn_2t+1, R21  
    
    lds R20, _znt
    lds R21, _znt+1
    sts  _zn_1t, r20      ;store new output as z(n-1) to  RAM
    sts  _zn_1t+1, r21
   
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
 
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }      
 

 
       
 
//1k-1.5kHz
int IIR4_f(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
    
                
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    ;*********first filter*****************input is x output is y********************    

    lds  R22, _b1_1f       ;load b1-first from RAM
    lds  R23, _b1_1f+1 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    mult_acc              ; b1*x   
    
    lds  R22, _b2_1f       ;load b2-first from RAM
    lds  R23, _b2_1f+1 
    lds  R20, _xn_1f     ;load x(n-1) from  RAM
    lds  R21, _xn_1f+1
    mult_acc              ; b2*x(n-1) 
    
    lds  R22, _b3_1f       ;load b3-first from RAM
    lds  R23, _b3_1f+1 
    lds  R20, _xn_2f     ;load x(n-2) from  RAM
    lds  R21, _xn_2f+1
    mult_acc              ; b3*x(n-2)
    
    lds  R22, _a2_1f       ;load -a2 - first from RAM
    lds  R23, _a2_1f+1 
    lds  R20, _yn_1f     ;load y(n-1) from  RAM
    lds  R21, _yn_1f+1
    mult_acc              ; -a2*y(n-1)  
    
    lds  R22, _a3_1f       ;load -a3 - first from RAM
    lds  R23, _a3_1f+1 
    lds  R20, _yn_2f     ;load y(n-2) from  RAM
    lds  R21, _yn_2f+1
    mult_acc              ; -a3*y(n-2)      
    
    lds  R20, _xn_1f       ;load x(n-1) -first from  RAM
    lds  R21, _xn_1f+1
    sts  _xn_2f, r20      ;store x(n-2) to  RAM
    sts  _xn_2f+1, R21 
    ldd   R20, Y+2           ;load input parameter x from stack
    ldd  R21, Y+3
    sts  _xn_1f, r20     ;store x(n-1) to  RAM
    sts  _xn_1f+1, R21 
    lds  R20, _yn_1f     ;load y(n-1) from  RAM
    lds  R21, _yn_1f+1
    sts  _yn_2f, R20     ;store y(n-2) to  RAM
    sts  _yn_2f+1, R21 
    sts  _yn_1f, r30     ;store new output as y(n-1) to  RAM
    sts  _yn_1f+1, r31

    ;*******second filter**************** input is y output is z**********************

    lds  R22, _b1_2f       ;load b1-second from RAM
    lds  R23, _b1_2f+1 
     mov  R20, r30        ;load output from first filter (y from r30-r31)
     mov  R21, r31

     sts  _ynf, R30
     sts  _ynf+1, R31    
     
     ;lds R20, _yn
    ; lds R21, _yn+1

     clr r27
     clr r24
     clr r30
     clr r31
     mult_acc             ; b1*y  
    
    lds  R22, _b2_2f       ;load b2-second from RAM
    lds  R23, _b2_2f+1 
    lds  R20, _yn_1_2f       ;load y(n-1) from  RAM
    lds  R21, _yn_1_2f+1
    mult_acc              ; b2*y(n-1) 
    
    lds  R22, _b3_2f       ;load b3-second from RAM
    lds  R23, _b3_2f+1 
    lds  R20, _yn_2_2f       ;load y(n-2) from  RAM
    lds  R21, _yn_2_2f+1
    mult_acc              ; b3*y(n-2)
    
    lds  R22, _a2_2f       ;load -a2-second from RAM
    lds  R23, _a2_2f+1 
    lds  R20, _zn_1f       ;load z(n-1) from  RAM
    lds  R21, _zn_1f+1
    mult_acc              ; -a2*z(n-1)  
     
    lds  R22, _a3_2f       ;load -a3-second from RAM
    lds  R23, _a3_2f+1 
    lds  R20, _zn_2f     ;load z(n-2) from  RAM
    lds  R21, _zn_2f+1
    mult_acc              ; -a3*z(n-2)     
    
    sts _znf, R30
    sts _znf+1,R31
    
    lds R22, _g4
    lds R23, _g4+1   
    mov R20, R30
    mov R21, R31
    mults       ; output *g3
    

    lds  R20, _yn_1_2f      ;load y(n-1) from  RAM
    lds  R21, _yn_1_2f+1
    sts  _yn_2_2f, r20      ;store y(n-2) to  RAM
    sts  _yn_2_2f+1, R21 
    lds   R20, _ynf            ;load input parameter y(n) from stack
    lds  R21, _ynf+1                                  
    
    sts  _yn_1_2f, r20      ;store y(n-1) to  RAM
    sts  _yn_1_2f+1, R21 
    lds  R20, _zn_1f      ;load z(n-1) from  RAM
    lds  R21, _zn_1f+1
    sts  _zn_2f, R20      ;store z(n-2) to  RAM
    sts  _zn_2f+1, R21  
    
    lds R20, _znf
    lds R21, _znf+1
    sts  _zn_1f, r20      ;store new output as z(n-1) to  RAM
    sts  _zn_1f+1, r21
   
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 
    
	
 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
     
 
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }
 
//>1.5KHz
int IIR2_5(int xx, int c)
// xx is the current input signal sample
// returns the current filtered output sample
{
    #asm
    
    push r20    ;save parameter regs
    push r21
    
    clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    lds  R22, _b5_1       ;load b1 from RAM
    lds  R23, _b5_1+1 
    ldd   R20, Y+2         ;load input parameter xx from stack
    ldd  R21, Y+3
    mult_acc            ; b1*xx  
    
    lds  R22, _b5_2       ;load b2 from RAM
    lds  R23, _b5_2+1 
    lds  R20, _x5n_1     ;load x(n-1) from  RAM
    lds  R21, _x5n_1+1
    mult_acc            ; b2*x(n-1) 
    
    lds  R22, _b5_3       ;load b3 from RAM
    lds  R23, _b5_3+1 
    lds  R20, _x5n_2     ;load x(n-2) from  RAM
    lds  R21, _x5n_2+1
    mult_acc            ; b3*x(n-2)
    
    lds  R22, _a5_2       ;load -a2 from RAM
    lds  R23, _a5_2+1 
    lds  R20, _y5n_1     ;load y(n-1) from  RAM
    lds  R21, _y5n_1+1
    mult_acc            ; -a2*y(n-1)  
    
    lds  R22, _a5_3       ;load -a3 from RAM
    lds  R23, _a5_3+1 
    lds  R20, _y5n_2     ;load y(n-2) from  RAM
    lds  R21, _y5n_2+1
    mult_acc            ; -a3*y(n-2)
    
    lds  R20, _x5n_1     ;load x(n-1) from  RAM
    lds  R21, _x5n_1+1
    sts  _x5n_2, r20      ;store x(n-2) to  RAM
    sts  _x5n_2+1, R21 
    ldd   R20, Y+2         ;load input parameter xx from stack
    ldd  R21, Y+3
    sts  _x5n_1, r20     ;store x(n-1) to  RAM
    sts  _x5n_1+1, R21 
    lds  R20, _y5n_1     ;load y(n-1) from  RAM
    lds  R21, _y5n_1+1
    sts  _y5n_2, R20     ;store y(n-2) to  RAM
    sts  _y5n_2+1, R21 
    sts  _y5n_1, r30     ;store new output as y(n-1) to  RAM
    sts  _y5n_1+1, r31 
    
    mov r20, r30
	mov r21, r31
	mov r22, r30
	mov r23, r31 

 	LD  R30,Y    ;load c to lsb result
	LDD  R31,Y+1    ;and msb result  

    clr r24
 	
	mult_acc
     
 
    pop r21             ;restore parameter regs
    pop r20
 
    #endasm 
 
 }                               
#pragma warn+    


void analysis(int xx)
{   
//      printf("xx=%d \t",xx);
//    printf("xx=%f \r\n",fix2float(xx));    
     
    
  //    p1=IIR2_1(xx,p1);//0-50         
      pfir=IIR4_fir(xx,pfir); //50-350   
      paf=IIR4_af(xx,paf);//350-500  
      p2=IIR4_s(xx,p2);//500-750            
      p3=IIR4_t(xx,p3);//750-1000   
      p4=IIR4_f(xx,p4);//1000-1500                             
 //     p5=IIR2_5(xx,p5);//>1500  
       
//       printf("p1=%f \r\n",p1); 
//       printf("p2=%f \r\n",fix2float(p2)); 
//       printf("p3=%f \r\n",fix2float(p3));  
//       printf("p4=%f \r\n",fix2float(p4));           
//       printf("p5=%f \r\n",fix2float(p5));
 
}         

unsigned char lookup()
{
   unsigned char ii;//[0-256]
   int jj,kk=0;
   unsigned char result,temp;
   float  differ[num_dic];
   int  tmp_array[size_dic];
   float diffeuclid[num_dic];
   unsigned char rangeindex[num_dic];   // store the code with large coefficients
   
   for (ii=0;ii<num_dic;ii++)
   {
     differ[ii]=0.0;      
     diffeuclid[ii]=0.0;
   }
   
   for( ii=0; ii < num_dic; ii++ ) 
   {                             
      for(jj=0;jj<size_dic;jj++)
      {
          tmp_array[jj]=dic[ii][jj];
      }
      differ[ii]=calcorrelation(tmp_array, word);
      printf("i:%d \t, differ :%f\r\n",ii,differ[ii]);
   }                
       
//   New added  04/30   (to find the small coefficient)           
   for(ii=0; ii<num_dic;ii++)
   {
       if(differ[ii]<0.01) 
       { 
        rangeindex[kk++]=ii;
       } 
   }                            
#ifdef CORRELATION
    result = find_min(differ);   
#endif    
    // kk is the number of the codes with large coefficients, the index of these codes is saved in rangeindex
#ifdef EUCLID    
   if (kk < 2)   
      result = find_min(differ);  //  case: only one code has large coefficients
   else                          //  in other case, use euclid distance
   {         
      for (ii=0; ii < kk; ii++)
	  {
	    printf("Euclidian\r\n");
	    temp = rangeindex[ii];
        diffeuclid[ii]=0.;
        for (jj=0; jj < size_dic; jj++)
	    {	
             diffeuclid[ii]=diffeuclid[ii]+abs(fix2float(dic[temp][jj]) - fix2float(word[jj]));
//		 diffeuclid[ii]=diffeuclid[ii]+abs(dic[temp][jj] - fix2float(word[jj]));
        }
      }
      result = find_min(diffeuclid);    //   Here confused. is unsigned char equal to int??
      result = rangeindex[result];

    }        
#endif    
//   New added 04/30 end            
    return result;
}            
      
unsigned char find_min(float a[num_dic])
{
  float smallest;
  unsigned char ii;
  unsigned char index;
     
     index =0;  
     smallest = a[0];
     
     for(ii=1; ii<num_dic;ii++)
     {
       if(a[ii]<smallest) 
       { 
       smallest=a[ii];
       index = ii;
       } 
     }   
     
     index++;    
  return index;
}              
// Calculate the average of the fingerprints
void get_average()
{
    int ii,jj;
    float tmp[size_dic];               
    for(ii=0;ii<size_dic;ii++)
       tmp[ii]= 0.0;
       
    for (ii=0;ii<num_dic;ii++)
    {
       for (jj=0;jj<size_dic;jj++)
       {
           tmp[jj] = tmp[jj] + fix2float( dic[ii][jj]);
       }
    }
    
    for(jj=0;jj<size_dic;jj++)            
    {
         tmp[jj] = tmp[jj] + fix2float(word[jj]);
         printf("%f,\r\n",tmp[jj]/(num_dic+1));
    }                
}

float calcorrelation(int x[size_dic], int y[size_dic])
{ 
	int ii;
	float meanx1=0, meanx2=0, meanxy=0, meany1=0, meany2=0;
	float r;
	for (ii=0; ii<size_dic; ii++)
	{
		meanx1 = meanx1 + fix2float(x[ii]);
		meanx2 = meanx2 + fix2float(x[ii])*fix2float(x[ii]);
		meany1 = meany1 + fix2float(y[ii]);
		meany2 = meany2 + fix2float(y[ii])*fix2float(y[ii]);
		meanxy = meanxy + fix2float(x[ii])*fix2float(y[ii]);
	}
	
	if (((size_dic*meanx2-meanx1*meanx1)==0) && ((size_dic*meany2-meany1*meany1)==0) )
	   r = 0;
	else 
	{	
	   r=(size_dic*meanxy-meanx1*meany1)*(size_dic*meanxy-meanx1*meany1)/(size_dic*meanx2-meanx1*meanx1)/(size_dic*meany2-meany1*meany1);
	   r=1-r;
	} 	
	  
    if (r < 0)
    {
         r = -r;
    }
	return r;
		

}