/********************************************************************************
The programme of the IIR filter (Butterworth type, 2 order).
Using INT2 to get the input signal.
Array x is the input signal from A/D, the length is 256, 32-bit floating point.
Array y the filter output signal, the length is 256, 32-bit floating point.
Array a and b are the coefficient of the IIR filter, the length is 3. 
*********************************************************************************/
#pragma CODE_SECTION(vect,"vect")

#include "stdio.h"
#include "math.h"
#define pi 3.1415927
#define IMR   *(pmem+0x0000)  
#define IFR   *(pmem+0x0001)  
#define PMST  *(pmem+0x001D)  
#define SWCR  *(pmem+0x002B)  
#define SWWSR *(pmem+0x0028) 
#define AL    *(pmem+0x0008) 

#define CLKMD  0x0058 /* clock mode reg*/

#define Len 256

double fs,nlpass,nlstop,nhpass,nhstop,a[3],b[3],x[Len],y[Len];

void biir2lpdes(double fs, double nlpass, double nlstop, double a[], double b[]);

unsigned int  *pmem=0;
ioport	unsigned char	port8002;
int in_x[Len];
int m = 0;
int intnum = 0;
double xmean=0;
int n=0;
int flag = 0;
int i,j,p,k=0;
double w2,w1,w0;

void cpu_init()
{
    *(unsigned int*)CLKMD=0x0;         //switch to DIV mode clkout= 1/2 clkin
     while(((*(unsigned int*)CLKMD)&01)!=0);
   *(unsigned int*)CLKMD=0x77ff;      //switch to PLL 
	
	PMST=0x3FA0;
	SWWSR=0x7fff;
	SWCR=0x0000;
	IMR=0;
	IFR=IFR;
} 

interrupt void int1()   
{
	in_x[m] = port8002;
	in_x[m] &= 0x00FF;
	m++;
	intnum = m;
	
	if (intnum == Len)
	{
		intnum = 0;
		
		xmean = 0.0;
		
		for (i=0; i<Len; i++)
		{
			xmean = in_x[i] + xmean;
		}		
		
		xmean = 1.0*xmean/Len;
		
		for (i=0; i<Len; i++)
		{
			x[i] = 1.0*(in_x[i] - xmean);
		}
		
		for (i=0; i<Len; i++)
	  	{ 
			  w2 = x[i]-a[1]*w1-a[2]*w0;
      	    y[i] = b[0]*w2+b[1]*w1+b[2]*w0;
      		  w0 = w1;
      		  w1 = w2;
      	}
		
		m=0;
		flag = 1;
	}
}

  void biir2lpdes(double fs, double nlpass, double nlstop, double a[], double b[])
  
  {
    int i,u,v;
	double wp,omp,gsa,t;
    wp=nlpass*2*pi;
	omp=tan(wp/2.0);
	gsa=omp*omp;
	for (i=0; i<=2; i++)
	  {
		u=i%2;
		v=i-1;
		a[i]=gsa*pow(2,u)-sqrt(2)*omp*v+pow(-2,u);
	  }
	
	for (i=0; i<=2; i++)
	  {	u=i%2;
		b[i]=gsa*pow(2,u);
	  }
	t=a[0];
	for (i=0; i<=2; i++)
	  {	a[i]=a[i]/t;
		b[i]=b[i]/t;
	  }
  }
 
void set_int()    
{
	asm(" ssbx intm");
	IMR=IMR|0x0004;  
	asm(" rsbx intm");  
}

void main(void)
{
	w2=w1=w0=0.0;
	cpu_init();
	
  	fs = 250000;
  	nlpass = 0.08;
	nlstop = 0.28;
	
	biir2lpdes(fs,nlpass,nlstop,a,b);

	set_int();

	for(;;)
	{
		if (flag == 1)
		{
		   flag = 0;    /* set breakpoint here */

		}
	}
}

void vect()
{
   asm(" .ref _c_int00");/*pseudoinstruction*/
   asm(" .ref _int1");

   asm(" b _c_int00");/* reset */  
   asm(" nop");
   asm(" nop");
   asm(" rete");        
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* int0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");  
   asm(" nop");			    /* int1 */
   asm(" nop");
   asm(" nop");
 //  asm(" rete");       /* int2 */
   asm(" b _int1");   
   asm(" nop");
   asm(" nop");
 //  asm(" nop");
   asm(" rete");      /* tint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* brint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* bxint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");      /* dmac0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* tint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* int3 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* hpint */
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* brint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* bxint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* dmac4 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");      /* dmac5 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
}