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

//unsigned int unstuff(unsigned char c, unsigned int i) {
//    static unsigned char last = 0;
//    if ((c == DLE) && (last == DLE)) {
//        last = 0;
//        return i;
//    } else if ((c == STX) && (last == DLE)){  //Recieved a DLE followed by a STX, possibly the start of a new packet mid-stream.
//        last = c;
//        return 2;
//    } else  //Just a normal character
//        last = c;
//        return i+1;
//    }


#ifdef KEYBOARD_TESTING
void __attribute__((__interrupt__, auto_psv)) _U1RXInterrupt(void) //Unpacks Maxon frames as bytes arrive.
{
        unsigned char c;
       __asm__ volatile ("push CORCON");
         c = U1RXREG;

         switch (c)
         {
             case '1':   MAXON_TETHER_POWER = 1; break;
             case 'q':  MAXON_TETHER_POWER = 0; break;

             case '2':   CTRL_BATT_POWER = 1; break;
             case 'w':  CTRL_BATT_POWER = 0; break;

             case '3':   CHARGE_ENABLE = 1; break;
             case 'e':  CHARGE_ENABLE = 0; break;

             case '4':   CAN_ENABLE = 1; break;
             case 'r':  CAN_ENABLE = 0; break;

             case '5':   MAXON_POWER = 1; Status.bits.MaxonPower = 1; break;
             case 't':  MAXON_POWER = 0; Status.bits.MaxonPower =0; break;

             case 'c': raw = 1;  break;
         }

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

#else



void __attribute__((__interrupt__, auto_psv)) _U1RXInterrupt(void) //Unpacks Maxon frames as bytes arrive.
{
    static unsigned char last = 0;
    unsigned char c;
    unsigned char *frame_ptr = (unsigned char *) &(Maxon_FrameA);
    unsigned int *frame_int_ptr = (unsigned int *) &(Maxon_FrameA);
   // char s[128];

    __asm__ volatile ("push CORCON");

    while (U1STAbits.URXDA) //Loop through all available characters that have been recieved and are in the FIFO
    {
        c = U1RXREG;

        frame_ptr[i_ctr] = c;

        if(i_ctr == 0) {  //Looking for first character of packet.
            last = c;
            if(c == DLE)
                i_ctr = 1;
        }
        else if ((c == STX) && (last == DLE) && (i_ctr==1)) { //Recieved a DLE followed by a STXthat isn't part of a pair of DLE's, start of packet
            last = c;
            i_ctr = 2;
        } else if ((c == DLE) && (last == DLE)) { //Don't advance index on second DLE in a row.
            last = 0;
        } else if(i_ctr>=2)
        {  //Normal character, advance index and store for future comparison
            last = c;
            i_ctr++;
        } 

        if((i_ctr > 4) && (i_ctr == 2 * Maxon_FrameA.Len + 4))  //Got all data, advance index to CRC bytes
        {
            i_ctr = sizeof (Maxon_FrameA) - 2;
        }


           if ((i_ctr > 3) && i_ctr == sizeof (Maxon_FrameA)) //Entire frame has been recieved and unstuffed.
        {
               last = 0;  //Ensure a DLE isn't carried over from packet to packet
            //Check CRC
            int crc;
            crc = CalcFieldCRC(&(frame_int_ptr[1]),Maxon_FrameA.Len+2);  
            if(Maxon_FrameA.CRC == crc)
            {
                if((Maxon_FrameA.Data[0] == 0) && (Maxon_FrameA.Data[1] == 0)) //Maxon reports it understood command
                  Maxon_CommError = RS232_EXCHANGE_VALID;
                else
                  Maxon_CommError = RS232_COMM_ERROR;
            }
            else
               Maxon_CommError = RS232_CRC_ERROR;

            i_ctr = 0;
        }


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

}

#endif

//void __attribute__((__interrupt__,auto_psv)) _U1RXInterrupt(void)
////void __attribute__((__interrupt__(__save__(CORCON)))) _U1RXInterrupt(void)
//{
//    unsigned char c;
//    static int first_time = 1;
//
//    __asm__ volatile ("push CORCON");
//
//    if(first_time)
//    {
//        first_time = 0;
//    }
//
//    c = U1RXREG;
//
//    _U1RXIF = 0; //Clear Interrupt
//    __asm__ volatile ("pop CORCON");
//
//}

int IMU_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)) _U2RXInterrupt(void) //Frames and unpacks IMU packets as bytes arrive.
//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;
    }


    while (U2STAbits.URXDA) //Loop through all available characters that have been recieved and are in the FIFO
    {

    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);
            }
        }

    }
    }
    _U2RXIF = 0; //Clear Interrupt

    __asm__ volatile ("pop CORCON");

}