#byte PORTB=0xF81
#byte TRISB=0xF93

#define LEDRED PIN_B0
#define LEDGREEN PIN_B1
#define LED_ON output_high
#define LED_OFF output_low

#define COTA_X 3
#define COTA_Y 3

// constantes y variables para el filtro digital
#define NZEROS 3
#define NPOLES 3
static float xv[NZEROS+1], yv[NPOLES+1];


unsigned long Sample_X=0, Sample_Y=0;
unsigned long long sstatex=0,sstatey=0;

signed long long accelerationx[2]={0,0}, accelerationy[2]={0,0};
signed long long velocityx[2]={0,0}, velocityy[2]={0,0};

unsigned int countx=0,county=0;

void init(void);
void calibrate(void);
void data_transfer(void);
void movement_end_check(void);
void position(void);
void ADC_GetAllAxis(void);
void filter(void);

void init(void)
{
   TRISB=0x40;
   
   setup_adc_ports(AN0_TO_AN2 | VSS_VDD);
   setup_adc(ADC_CLOCK_INTERNAL);
   
   LED_OFF(LEDGREEN);
   LED_ON(LEDRED);
  
   delay_ms(1000);
   calibrate();
   
   usb_cdc_init();
   usb_init();
}

void calibrate(void)
{
   unsigned int count1=0;
   sstatex=0;
   sstatey=0;
   
   while(count1<200)
   {
      ADC_GetAllAxis();
      sstatex += Sample_X;
      sstatey += Sample_Y;
      count1++;
   }
   
   sstatex /= 200;
   sstatey /= 200;
}

void position(void)  
{
   unsigned int count2=0;
   
   accelerationx[1] = 0;
   accelerationy[1] = 0;
   
   while(count2<64)
   {
      ADC_GetAllAxis();
      accelerationx[1] += Sample_X; //filtering routine for noise attenuation
      accelerationy[1] += Sample_Y; //64 samples are averaged. The resulting 
                                    //average represents the acceleration of an instant
      count2++;                                       
   }                                // 64 sums of the acceleration sample
   accelerationx[1] /= 64;          // division by 64
   accelerationy[1] /= 64;
   
   filter();
   
   accelerationx[1] = (accelerationx[1]-(sstatex+4))/2;
   accelerationy[1] = (accelerationx[1]-(sstatey+4))/2;
   
   if ((accelerationx[1] <=COTA_X)&&(accelerationx[1] >= -COTA_X))   //Discrimination window applied
      accelerationx[1] = 0;                                          // to the X axis acceleration variable
    
   if ((accelerationy[1] <=COTA_Y)&&(accelerationy[1] >= -COTA_Y)) 
      accelerationy[1] = 0;
   
   //integration
   velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0]) /2);     
   velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0]) /2); 
   
   
   accelerationx[0] = accelerationx[1];  //The current acceleration value must be sent to the previous acceleration 
   accelerationy[0] = accelerationy[1];  //variable in order to introduce the new acceleration value.
   velocityx[0] = velocityx[1];          //Same done for the velocity variable
   velocityy[0] = velocityy[1];
   
   movement_end_check();
}

void ADC_GetAllAxis(void)
{
   set_adc_channel(0);
   delay_us(20);
   Sample_X = read_adc();
   
   set_adc_channel(1);
   delay_us(20);
   Sample_Y = read_adc();
}

void movement_end_check(void)   
{
   if (accelerationx[1]==0)   //we count the number of acceleration samples that equals cero
      countx++;
   else
      countx =0;
   
   if (countx>=40)          //if this number exceeds 25, we can assume that velocity is cero
   {
      velocityx[1] = 0;
      velocityx[0] = 0;
   }
   
   if (accelerationy[1]==0)   //we do the same for the Y axis
      county++;
   else
      county =0;
   
   if (county>=40)
   {
      velocityy[1] = 0;
      velocityy[0] = 0;
   }
}

void data_transfer(void)
{
   printf(usb_cdc_putc,"%li\n\r",velocityx[1]);
}

#define GAIN   4.553605266e+03

void filter()
{
   xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; 
        xv[3] = accelerationx[1] / GAIN;
        yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; 
        yv[3] =   (xv[0] + xv[3]) + 3 * (xv[1] + xv[2])
                     + (  0.7776385602 * yv[0]) + ( -2.5282312191 * yv[1])
                     + (  2.7488358092 * yv[2]);
        accelerationx[1] = yv[3];

        //accelerationx[1]
}