#include <stdio.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.h"
#include "extern.h"
#include "imu.h"
#include "nortek.h"


int Checksum(int *phBuff,int n)
{
int i;
int hChecksum = 0xb58c;
for (i=0; i<n; i++)
hChecksum += phBuff[i];
return hChecksum;
}



void __attribute__((__interrupt__,auto_psv)) _U1RXInterrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _U1RXInterrupt(void)
{
    unsigned char c;
    static unsigned char *packet_ptr;
    static int i = 0;
    static int first_time = 1;

    __asm__ volatile ("push CORCON");

    if(first_time)
    {
        packet_ptr = (unsigned char *) &(Nortek_DataA);
        Nortek_Buff = 1;  //writing int BuffA, so Buff B is valid
        first_time = 0;
    }
    
    c = U1RXREG;

        if ((i == 0) && (c == 0xA5)) {
        packet_ptr[i] = c;
        i = 1;
        _U1RXIF = 0; //Clear Interrupt
        __asm__ volatile ("pop CORCON");
        return;
    }

    if ((i == 1) && (c == 0x10)) {
        packet_ptr[i] = c;
        i = 2;
        _U1RXIF = 0; //Clear Interrupt
        __asm__ volatile ("pop CORCON");
        return;
    }

    if (i >= 2) {
        packet_ptr[i] = c;
        i++;
    }


    if(i==sizeof(Nortek_DataA))
    {
     i = 0;
     if(Checksum((int *) packet_ptr,11) == ((Nortek_Packet *) packet_ptr)->CheckSum)  //If valid checksum, swap buffers.
        {
         if(Nortek_Buff == 1)  //We've been writing into Buffer A.
          {
            //iTemp2 = Nortek_DataA.CheckSum;
            Nortek_Buff = 0;
            packet_ptr = (unsigned char *) &(Nortek_DataB);
          }
         else                 //We've been writing into Buffer B.
          {
            //iTemp2 = Nortek_DataB.CheckSum;
            Nortek_Buff = 1;
            packet_ptr = (unsigned char *) &(Nortek_DataA);
          }
        }
    }

    _U1RXIF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}




void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _U2RXInterrupt(void)
{
    unsigned char c;
    static unsigned char *packet_ptr;
    static int i = 0;
    static int first_time = 1;
    static unsigned int crc;

    __asm__ volatile ("push CORCON");

    if(first_time)
    {
        packet_ptr = (unsigned char *) &(IMU_DataA);
        IMU_Buff = 1;  //writing int BuffA, so Buff B is valid
        first_time = 0;
    }


    c = U2RXREG;

    if(i==0)
    {
        if(c==0xFA)
        {
        packet_ptr[i] = c;
        i=1;
        crc = 0;
        }
    }
    else
    {
      packet_ptr[i] = c;
      crc  = (unsigned char)(crc >> 8) | (crc << 8);
      crc ^= packet_ptr[i];
      crc ^= (unsigned char)(crc & 0xff) >> 4;
      crc ^= crc << 12;
      crc ^= (crc & 0x00ff) << 5;
      i++;
    }


    if(i>=sizeof(IMU_DataA))
    {
        i = 0;
        if(crc == 0)
        {
         if(IMU_Buff == 1)  //We've been writing into Buffer A.
          {
            IMU_Buff = 0;
            packet_ptr = (unsigned char *) &(IMU_DataB);
          }
         else                 //We've been writing into Buffer B.
          {
            IMU_Buff = 1;
            packet_ptr = (unsigned char *) &(IMU_DataA);
          }
        //AD1CON1bits.ASAM = 1; //Auto-sample start bit set if valid CRC.  This is cleared after ADC is read.
        }
        AD1CON1bits.ASAM = 1; //Auto-sample start bit set if valid CRC.  This is cleared after ADC is read.
    }

    _U2RXIF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}