/*
    iir_i1.c - IIR direct form II biquads filter
               in Fixed-point implementation
*/

#include <intrindefs.h>
#define  SCALE 1

void iir(int *x, int Nx, int *y, int *coef, int Ns, int *w)
{      
    int i,j,n,m,k,l;
    int temp;
    long w_0;
        
    m=Ns*5;             /* Setup for circular buffer coef[] */
    k=Ns*2;             /* Setup for circular buffer d[] */

    for (j=0,l=0,n=0; n<Nx; n++)        /* IIR filter */ 
    {
        w_0 = (long)x[n]<<(15-SCALE);   /* Q14 input (scaled) */ 
        for (i=0; i<Ns; i++)
        {    
            w_0 = _smas(w_0,*(w+l),*(coef+j)); j++; l=(l+Ns)%k;
            w_0 = _smas(w_0,*(w+l),*(coef+j)); j++;
       	    temp = *(w+l);
	    *(w+l) = w_0>>15;                  
	    w_0 = _lsmpy(     temp,*(coef+j)); j++; 
 	    w_0 = _smac(w_0,*(w+l),*(coef+j)); j++; l=(l+Ns)%k; 
            w_0 = _smac(w_0,*(w+l),*(coef+j)); j=(j+1)%m; l=(l+1)%k;
        }
        y[n] = w_0>>(15-SCALE);         /* Q15 format output */
    }
}