/*********************************************************************************
The programme of the FIR filter.
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 is the signal out of filter, the length is 256, 32-bit floating point.
Array h is the coefficient of the FIR filter, the length is 51, 51 order filter.
**********************************************************************************/
#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
#define FLen 51

double npass,h[FLen], x[Len], y[Len], xmid[FLen];
void firdes (double npass);

unsigned int  *pmem=0;
ioport	unsigned char	port8001;
int in_x[Len];
int m = 0;
int intnum = 0;
double xmean=0;
int i=0;
int flag = 0;
double fs,fstop,r,rm;
int i,j,p,k=0;

void cpu_init()
{
	//asm(" nop");
	//asm(" STM #0, CLKMD");   
	//asm(" STM #0, CLKMD");   
	//asm(" rpt  #0ffffh");
	//asm(" nop");
	//asm(" STM #0x97ff, CLKMD");
	
   *(unsigned int*)CLKMD=0x0;         //switch to DIV mode clkout= 1/2 clkin
     while(((*(unsigned int*)CLKMD)&01)!=0);
   *(unsigned int*)CLKMD=0x27ff;      //switch to PLL X 10 mode
	
	PMST=0x3FA0;
	SWWSR=0x7fff;
	SWCR=0x0000;
	IMR=0;
	IFR=IFR;
} 

interrupt void int2()   
{
	in_x[m] = port8001;
	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] = (double)(in_x[i] - xmean);
		}
		
		for (i=0; i<Len; i++)
		{
			for (p=0; p<FLen; p++)
   			{
   				xmid[FLen-p-1] = xmid[FLen-p-2];
   			}
   			
   			xmid[0] = x[i];
    
			r = 0;
			rm= 0; 
			for (j=0; j<FLen; j++)
			{	
 		 		r  = xmid[j] * h[j];
				rm = rm + r;
   			}
			y[i] = rm;
   		}
		
		m=0;
		flag = 1;
		
	}
}

  void firdes(double npass)
  {
  	int t;
   	for (t=0; t<FLen; t++)
  	{
  		h[t] = sin((t-(FLen-1)/2.0)*npass*pi)/(pi*(t-(FLen-1)/2.0));
  	}
  	if (t == ((FLen-1)/2)) h[t]=npass;
  }
  
 
void set_int()    
{
	asm(" ssbx intm");
	IMR=IMR|0x0004;  
	asm(" rsbx intm");  
}

void main(void)
{
	cpu_init();
	
  	fs = 250000;
  	fstop = 20000;
  	npass = fstop/fs;
  	
  	for (i=0; i<FLen; i++)
  	{
  		xmid[i]=0;
  	}
  	
  	firdes(npass);

	set_int();

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

		}
	}
}

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

   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");      /* int1 */
   asm(" nop");			
   asm(" nop");
   asm(" nop");
   asm(" b _int2");   /* int2 */
   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");
}