//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>

//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   } 

// filter scaling
#define scaleFactor 16
#define scaleFactorShift 4

//float-fix conversion macros
#define float2fix(a) ((int)((a)*256.0)) 
#define fix2float(a) ((double)(a)/256.0)

//MAC macro
#define macfixM(y,x,b) (((int)((((long)(x))*((long)(b)))>>8)) + (y))
  
//IIR state variables
int b1,b2,b3,a2,a3 ;
int xn, xn_1, xn_2 ; 
int yn_1, yn_2  ;
 
//========================================================   
//second order IIR -- "Direct Form II Transposed"
//  a(1)*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); 
	yy = yy>>scaleFactorShift ; // divide by 16 = a(1)
    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*scaleFactor) ;
    b2 = float2fix(0.195*scaleFactor) ;
    b3 = float2fix(0.098*scaleFactor) ;                
    a2 = float2fix(0.943*scaleFactor) ;   //note that sign is negated from design input
    a3 = float2fix(-0.333*scaleFactor) ;  //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

  while(1)
  begin
     
     //impulse response of IIR2 filter 
     xn=0x1000;  //test impulse amplitude = 16
     for (i=0;i<16;i++)
     begin
        out = IIR2(xn) ;
        printf("%f %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 = IIR2(xn) ;
    TCCR1B = 0;
    printf("IIR cycles=%d\n\r",TCNT1)  ;
    
    //============================================
      
  while(1); // halt
  end
end