//IIR filter
//using direct form  
//normalized to unity gain, and with a(1)=1
// a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
//                       - a(2)*y(n-1) - ... - a(na+1)*y(n-na)    
// Bruce Land -- Cornell University -- June 2008
         
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include "uart.h"

//set up the debugging utility ASSERT
#define __ASSERT_USE_STDERR
#include <assert.h>
#include "uart.h"
// UART file descriptor
// putchar and getchar are in uart.c
FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);

//I like these definitions
#define begin {
#define end   } 

//float-fix conversion macros
#define float2fix(a) ((int)((a)*256.0)) 
#define fix2float(a) ((double)(a)/256.0)
  
//IIR state variables
int b1,b2,b3,a2,a3, a4, a5 ;
int xn, xn_1, xn_2, xn_3, xn_4 ; 
int yn_1, yn_2, yn_3, yn_4  ;
 
//========================================================   
//second order IIR -- "Direct Form II Transposed"
//  y(n) = b(1)*x(n) + b(2)*x(n-1) +  b(3)*x(n-2)
//                   - a(2)*y(n-1) -  a(3)*y(n-2) 
//assumes a(1)=1

int IIR2(int xx)
begin
    int yy = 0 ; 
    // sum the 5 terms: yy += xx*coeff 
	// and update the state variables
	// as soon as possible
	yy = macfix(yy,xn_2,b3);
	xn_2 = xn_1;
	yy = macfix(yy,xn_1,b2);
	xn_1 = xx;
	yy = macfix(yy,xx,b1);
	yy = macfix(yy,yn_2,a3); 
	yn_2 = yn_1;
	yy = macfix(yy,yn_1,a2); 
    yn_1 = yy;
    return yy;
end 

//======================================================== 
// Second order Butterworth lowpass
// xx is the current input signal sample
// returns the current filtered output sample
// Simplification: b(1)*x(n)+b(2)*x(n-1)+b(3)*x(n-2) = 
// b(1)*x(n)+2*b(1)*x(n-1)+b(1)*x(n-2) =
// b(1)* (x(n)+(x(n-1)<<1)+x(n-2))
int Butter2low(int xx)
begin
    int yy = 0 ; 
    // sum the 5 terms: yy += xx*coeff 
	// and update the state variables
	// as soon as possible
	
	yy = macfix(yy, (xx+(xn_1<<1)+xn_2), b1);
	xn_2 = xn_1;
	xn_1 = xx;
	yy = macfix(yy,yn_2,a3); 
	yn_2 = yn_1;
	yy = macfix(yy,yn_1,a2); 
    yn_1 = yy;
    return yy;
end 

//======================================================== 
// Second order Butterworth highpass
// xx is the current input signal sample
// returns the current filtered output sample
// b(1)*x(n)+b(2)*x(n-1)+b(3)*x(n-2) = 
// b(1)*x(n)-2*b(1)*x(n-1)+b(1)*x(n-2) =
// b(1)* (x(n)-(x(n-1)<<1)+x(n-2))
int Butter2high(int xx)
begin
    int yy = 0 ; 
	yy = macfix(yy, (xx-(xn_1<<1)+xn_2), b1);
	xn_2 = xn_1;
	xn_1 = xx;
	yy = macfix(yy,yn_2,a3); 
	yn_2 = yn_1;
	yy = macfix(yy,yn_1,a2); 
    yn_1 = yy;
    return yy;
end 

//======================================================== 
// Second order Butterworth bandpass
// xx is the current input signal sample
// b(1)*x(n)+b(2)*x(n-1)+b(3)*x(n-2) = 
// b(1)*x(n)+0*b(1)*x(n-1)-b(1)*x(n-2) =
// b(1)* (x(n)-x(n-2))
int Butter2band(int xx)
begin
    int yy = 0 ; 	
	yy = macfix(yy, (xx-xn_2), b1);
	xn_2 = xn_1;
	xn_1 = xx;
	yy = macfix(yy,yn_2,a3); 
	yn_2 = yn_1;
	yy = macfix(yy,yn_1,a2); 
    yn_1 = yy;
    return yy;
end 

//======================================================== 
// Fourth order Butterworth bandpass
// xx is the current input signal sample
// input part simplifies to b1*(xx-2*x(n-2)+x(n-4)) 
int Butter4band(int xx)
begin
	int yy = 0 ; 	
	yy = macfix(yy, (xx-(xn_2<<1)+xn_4), b1);
	xn_4 = xn_3;
	xn_3 = xn_2;
	xn_2 = xn_1;
	xn_1 = xx;
	yy = macfix(yy,yn_4,a5); 
	yn_4 = yn_3;
	yy = macfix(yy,yn_3,a4); 
	yn_3 = yn_2;
	yy = macfix(yy,yn_2,a3); 
	yn_2 = yn_1;
	yy = macfix(yy,yn_1,a2); 
    yn_1 = yy;
    return yy;
end
//========================================================  

int main(void)
begin 
	char i;
	signed int junk, out;    
      
  	// init the UART -- uart_init() is in uart.c
  	uart_init() ;
  	stdout = stdin = stderr = &uart_str ;
  	fprintf(stdout,"Starting IIR impulse...\n\r") ; 
  
  	//2-pole butterworth lowpass, cutoff=0.25 (1/4 Nyquist freq), gain=1 
 //   b1 = float2fix(0.098) ;
 //   b2 = float2fix(0.195) ;
 //   b3 = float2fix(0.098) ;                
 //   a2 = float2fix(0.943) ;   //note that sign is negated from design input
 //   a3 = float2fix(-0.333) ;  //note that sign is negated from design input 
    
    //2-pole butterworth lowpass, cutoff=0.1 (1/10 Nyquist freq), gain=1 
//     b1 = float2fix(0.0201) ;
//     b2 = float2fix(0.0402) ;
//     b3 = float2fix(0.0201) ;                
//     a2 = float2fix(1.561) ;   //note that sign is negated from design input
//     a3 = float2fix(-.6414) ;  //note that sign is negated from design input

//2-pole butterworth bandpass, cutoff=[0.2 0.4] 
//   b1 = float2fix(0.245) ;
//   b2 = float2fix(0.0) ;
//   b3 = float2fix(-0.245) ;                
//   a2 = float2fix(0.933) ;   //note that sign is negated from design input
//   a3 = float2fix(-0.510) ;  //note that sign is negated from design input

//4-pole butterworth bandpass, cutoff=[0.25 0.35]
  b1 = float2fix(0.0201) ;               
  a2 = float2fix(2.119) ;   //note that sign is negated from design input
  a3 = float2fix(-2.695) ;  //note that sign is negated from design input
  a4 = float2fix(1.692) ;  //note that sign is negated from design input
  a5 = float2fix(-0.6414) ;  //note that sign is negated from design input

  while(1)
  begin
     
     //impulse response of IIR2 filter 
     xn=0x1000;  //test impulse amplitude = 16
     for (i=0;i<16;i++)
     begin
        out = Butter4band(xn) ;
        printf("%2.3f %4x\n\r", fix2float(out), out) ; 
        xn=0 ;
        
     end  
    //============================================
    //TCCR1B = 1 ;
    //TCNT1 = 0;
   // junk = macfix(0x0100,0x0100,0x0100 ) ;
   // TCCR1B = 0;
   // printf("macfix cycles=%d\n\r",TCNT1)  ;  
    
    //============================================
    TCCR1B = 1 ;
    TCNT1 = 0;
    junk = Butter4band(xn) ;
    TCCR1B = 0;
    printf("IIR cycles=%d\n\r",TCNT1)  ;
    
    //============================================
      
  while(1); // halt
  end
end