#include "LPC22xx.H"    // LPC2214 MPU Register
#include "stdio.h"              // For Used Function printf
#include "math.h"
#include "stdlib.h"
//------------------------------
// Macros for Interrupt Nesting
#define IENABLE                      /* Nested Interrupts Entry */   \
  __asm { MRS     LR, SPSR      }    /* Copy SPSR_irq to LR     */   \
  __asm { STMFD   SP!, {LR}     }    /* Save SPSR_irq           */   \
  __asm { MSR     CPSR_c, #0x1F }    /* Enable IRQ (Sys Mode)   */   \
  __asm { STMFD   SP!, {LR}     }    /* Save LR                 */   \

#define IDISABLE                      /* Nested Interrupts Exit */   \
  __asm { LDMFD   SP!, {LR}     }     /* Restore LR              */   \
  __asm { MSR     CPSR_c, #0x92 }     /* Disable IRQ (IRQ Mode)  */   \
  __asm { LDMFD   SP!, {LR}     }     /* Restore SPSR_irq to LR  */   \
  __asm { MSR     SPSR_cxsf, LR }     /* Copy LR to SPSR_irq     */   \


//----------------------------------------

extern void init_serial0 (void);                                                        // Used Initial UART0 From "Serial0.c"
//CCLK=12xM=12x5=60MHZ
//PCLK=CCLK (VPBDIV=0x00)

void IRQ_Match0_timer0(void)__irq;//ngat Timer0
void IRQ_Capture_timer1(void)__irq;//ngat for CAP pin 1.1 11

//*************************************************************
//declare for Filter fs=1kHz,fc=40Hz,butterWorth low pass order=4

const  float Numerator[]={1,1,1,1,1};
const  float Denominator[]={1,-3,4,-2,0};                                                  
//*************************************************************

void Low_pass_filter(float *NUM,float *DEN,float *W,float data_update);
//===========
//flag for define filter for which chanel data changing
unsigned char flag_filter_1=1;//cho kenh ADXL1
unsigned char flag_filter_2=1;//cho kenh ADXL2

//=================
//read ADC
unsigned long int read_adc(unsigned int num_adc);
void outs(unsigned int datlong,float *p);
void delay(unsigned long int);  // Delay Time Function
float voltx;                    // ADC Result Volt
float volty;
float ax,ay,a,unit_x,unit_y;
unsigned int offsetx,offsety;
float vectx[1000]={0};
float vecty[1000]={0};
float samplex1[5]={0};
float sampley1[5]={0};
//for ADXL 2
unsigned char flag,chanel;
float asisx[20]={0};
float asisy[20]={0};
unsigned long tempx,tempy;
unsigned countx,county;
//======================
unsigned char count;
unsigned int dat_count,somau;
unsigned int value;
float temp;
//======ham chinh==========
//ADXL header VCC  ST x y gnd
int main(void)
{

unsigned char dem =20;

init_serial0(); 
// Initial UART0 = 9600,N,8,1
//int timer0
T0TCR=0x3;//TC and PR count enable
T0PR=59;//tan so dem cua TC = PCLK/(PR+1)=1MHz-> 1us tang 1 don vi
T0MCR=0x3;//interrupt match0 enable and TC reset when match , va giu TC ko dem cho toi khi bit 1 bang 0
T0MR0=1000;//1ms for TC reaches to T0MR0 and occur match0
//int timer1    for Capture pin
T1TCR=0x3;//TC and PR count enable
T1PR=59;//tan so dem cua TC = PCLK/(PR+1)=1MHz-> 1us tang 1 don vi
T1TCR=0x1;//cho phep TC dem
//T1CCR=0x0000002D;//interrup capture 1.0 1.1   when rising edge
T1CCR=0x00000005;//     interrup capture 1.0 when rising edge
//int flag
flag=0;
chanel=0;
//interrupt
//-----------
VICVectCntl0=0x24;//choose Source Int Timer0=4, and enable IRQ slot
VICVectAddr0=(unsigned) IRQ_Match0_timer0;
VICIntEnable|=1<<4;//enable Timer0 interrupt source
//-----------
VICVectCntl1=0x25;//choose Source Int Timer1=5, and enable IRQ slot
VICVectAddr1=(unsigned) IRQ_Capture_timer1;
VICIntEnable|=1<<5;//enable Timer1 interrupt source

  //chon adc.2 = P0.10 va adc.3=P0.12  
  //p0.12= Ax gia toc theo truc x
  //P0.10=Ay gia toc theo truc y         
  //truc X : unit_x=0.669Volt-------->1g : Range: 0.8      1.470       2.14 Volt
  //truc Y : unit_y=0.569Volt-------->1g : Range: 0.822    1.391       1.96 Volt

 

//int starting VIA
PINSEL1|=0x15400000; //cho phep 8 kenh ADC
PINSEL0|=0x00A00005;//capture 1.0 and 1.1 pin for timer1
offsetx=456;//ung voi 1.47 volt*1024/3.3
offsety=431;//ung voi 1.391 volt*1024/3.3
unit_x=0.669;
unit_y=0.569;
//count=100;
somau=0;
dat_count=0;
T0TCR=0x1;//cho phep TC dem

while(1) {  
        //thuc hien ham loc     real time
        // for ADXL1
        Low_pass_filter(Numerator,Denominator,samplex1,voltx);
        vectx[somau]=temp;
        Low_pass_filter(Numerator,Denominator,sampley1,volty);
        vecty[somau]=temp;
}
   
 
}
//=====================end main=======================
//==============interupt function=============
 void IRQ_Match0_timer0(void)__irq
{                   T0IR=1;      //xoa co ngat MR0 de cho match0 tiep theo xay ra
           
                        voltx=(float)read_adc(3);       //gia toc theo truc x
                        volty=(float)read_adc(2);       //gia toc theo truc y

                //      ax=(voltx-offsetx)*3.300/unit_x/1024.000-0.1 ; //(hieu chinh 0.1);
                  //    ay=(volty-offsety)*3.300/unit_y/1024.000 -0.1;//hieu chinh 0.1
                        dat_count++;
                        somau++;
                    if(somau>20)
                        {
        //              IENABLE; // allow nested interrupts

                        somau=0;
                        printf("\n vecto theo truc x\n");
                        outs(20,vectx);
                        printf("\n vecto theo truc y\n");
                        outs(20,vecty);
                        printf("already");
                        IDISABLE;// disable interrupt nesting
                        }           
                       
                 
        VICVectAddr = 0x00000000;       // Reset Global Interrupt and return
                       
}
//==========Capture Interupt=========
void IRQ_Capture_timer1(void)__irq
{
flag=~flag;
        if(flag!=0)//rising edge
        {
                if(chanel==0)
                {
                 tempx=T1CR0;
                 T1CCR= 0x00000006;//enable falling edge interupt,and disable rising edge on chanel0(cap1.0)   
                 T1IR|=(1<<4);//reset Interupt cap1.0
                }
                else
                {
                 tempy=T1CR1;
                 T1CCR= 0x00000030;//enable falling edge interupt,and disable rising edge on chanel1(cap1.1)   
                 T1IR|=(1<<5);//reset Interupt cap1.1
                }
         }
        else  //falling edge
        {
               
                if(chanel==0)
                {
                 chanel=1;
                 asisx[countx++]=(T1CR0>tempx)?(T1CR0-tempx):(0xFFFFFFFF-tempx+T1CR0+1);
                 T1CCR= 0x00000028;//enable rising edge interupt,and disable rising edge on chanel1(cap1.1)     
                 T1IR|=(1<<4);//reset Interupt cap1.0
                }
                else
                {
                 chanel=0;
                 asisy[county++]=(T1CR1>tempy)?(T1CR1-tempx):(0xFFFFFFFF-tempy+T1CR1+1);
                 T1CCR= 0x00000005;//enable rising edge interupt,and disable rising edge on chanel0(cap1.0)            
                 T1IR|=(1<<5);//reset Interupt cap1.1
                }

        }
VICVectAddr = 0x00000000;       // Reset Global Interrupt and return

       
}
//==============end interrupt===========
//read adc
unsigned long int read_adc(unsigned int num_adc)
{       //ADC Clock = VBP(PCLK) / 7
        //Busrt = 1 = Conversion Continue
        //CLKS = 000 = 10Bit : 11 Cycle Clock Conversion
        //PDN = 1 = Active ADC Module
        //EDGE = 0 = Conversion on Falling Edge
        //START = 001 = Start Conversion Now
unsigned int val;
unsigned long int temp;
  ADCR  = 0x00210D00 |(1<<num_adc);
   
            ADCR  |= 0x01000000;                   //Bat dau chuyen doi
                do
        {
                          val=  ADDR;
                }        
        while ((val & 0x80000000) == 0);    // Wait ADC Conversion Complete and clear DONE bit     
            val = (val >> 6) & 0x03FF;
                temp=val;
       
            //temp=temp*3.3/1023/100.0;
return (temp);
                   
 }

//========end function=================
void delay(unsigned long int count1)
{
  while(count1 > 0) {count1--;}                                                         // Loop Decrease Counter       
}
//xuat port
void outs(unsigned int datlong, float *p)
{
unsigned int t;
float d;
unsigned char k;
k=1;
for(t=0;t<datlong-1;t++)
{

p++;
d=(*p-offsetx)*3.300/unit_x/1024.000/1.017;
printf("%1.3f;",d);
}
}
//IIR filter designed by matlab order =4
//thoi gian thuc hien 5->8us cho/1mau
void Low_pass_filter(float *NUM,float *DEN,float *W,float data_update)
{
         unsigned char i;
        for(i=4;i>0;i--)
        {
        *(W+i)=*(W+i-1);
        }
        *W=data_update-DEN[1]*W[1]-DEN[2]*W[2]-DEN[3]*W[3]-DEN[4]*W[4];
        temp=NUM[0]*W[0]+NUM[1]*W[1]+NUM[2]*W[2]+NUM[3]*W[3]+NUM[4]*W[4];

}