/********************************************************************************
The programme of the Correlation Algorithm.
Using INT2 to get the input signal.
Array x, in first step, is the input signal produced by programme,
in next step, is the input signal get from A/D,
the length is 128, 32-bit floating point.
Array y, in first step, is the input signal produced by programme,
in next step, is the input signal get from A/D,
the length is 128, 32-bit floating point.
Array cor is the Correlation result, the length is 255, 32-bit floating point.
*********************************************************************************/
#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 Length  128                 /* input array x,y length */
#define Lengthcor  2*Length-1       /* ouput array cor length */

int i,k,j,mode;
double sum, t, temp, xavg;
double x[Length],y[Length];
double cor[Lengthcor];  
int xm;

void Cor_caculation( double x[Length],double y[Length],double cor[Lengthcor]);

unsigned int  *pmem=0;
ioport unsigned char port8008;
int in_x[Length];
int m = 0;
int intnum = 0;

int flag = 0;

void cpu_init()
{
   *(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
 
 PMST=0x3FA0;
 SWWSR=0x7fff;
 SWCR=0x0000;
 IMR=0;
 IFR=IFR;
}

interrupt void int2()  
{
 in_x[m] = port8008;
 in_x[m] &= 0x00FF;
 m++;
 intnum = m;
 
 if (intnum == Length)
 {
  intnum = 0;
  
  xavg = 0.0;
  
  for (i=0; i<Length; i++)
  {
   xavg = in_x[i] + xavg;
  }  
  
  xavg = xavg/Length;
  
  for (i=0; i<Length; i++)
  {
   x[i] = 1.0*(in_x[i] - xavg);
   y[i] = x[i];
  }
  
  Cor_caculation(x,y,cor);
  
  m=0;
  flag = 1;
  IMR=0x0000;
 }
}

void Cor_caculation( double x[Length],double y[Length],double cor[Lengthcor])
{
   for(k=0; k<Length; k++) 
   {
      sum=0;
     
      for(j=0; j<=Length-1-k; j++)
      {
         t = x[j+k]*y[j];
         sum = sum+t;
      }

      if(mode==0)
      {
         cor[Length-1-k] = sum/(Length-k);
      }

      else
      {
         cor[Length-1-k] = sum/Length;
      }
   }

   for(k=0; k<=Length-1; k++) 
   {
      sum=0;

      for(j=0; j<=Length-1-k; j++)
      {
         t = x[j]*y[j+k];
         sum = sum+t;
      }

      if(mode==0)
      {
         cor[Length-1+k] = sum/(Length-k);
      }

      else
      {
         cor[Length-1+k] = sum/Length;
      }
   }
}
 
void set_int()   
{
 asm(" ssbx intm");
 IMR=IMR|0x0002; 
 asm(" rsbx intm"); 
}

void main(void)
{
 k = 0; sum =0;
    t = 0; temp = 0;

/*************************************************
 when mode=1, result is biased estimate;
    when mode=0, result is unbiased estimate
*************************************************/                              
  
 mode = 1;
 
 cpu_init();

 for(i=0; i<Length; i++)   /*Initialize*/
    {
           x[i]=0;
           y[i]=0;
           cor[i]=0;
 }

 for (i=Length; i<Lengthcor; i++)
    {
        cor[i] = 0;
    }

 for(i=0; i<Length; i++)  /*Input x,y*/
 {
           x[i] = sin(2*pi*i/(Length-1));
            y[i] = sin(2*pi*i/(Length-1));
    }

 Cor_caculation(x,y,cor);
 
 i++;              /* set breakpoint here */

 set_int();

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

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