#include <stdio.h>
#include "config.h"
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "extern.h"
#include "libq.h"
#include "../CommonCode/conv.h"
#include "CalibrationConstants.h"
#include "../CommonCode/types.h"
#include "../CommonCode/crc.h"

#ifdef DEBUG

/*
void __attribute__((__interrupt__, auto_psv)) _U2RXInterrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _U2RXInterrupt(void)
{
    char c;
    __asm__ volatile ("push CORCON");
    c = U2RXREG;
    switch (c) {

        case 'c': //Set display to raw counts
            raw = 1;
            break;

        case 'e': //Set display to engineering units
            raw = 0;
            break;

        default:
            break;
    }


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

}
*/
#endif




void __attribute__((__interrupt__, auto_psv)) _U1RXInterrupt(void)
//void __attribute__((__interrupt__(__save__(CORCON)))) _U1RXInterrupt(void)
{
    static int state;
    unsigned int crc;
    char* char_ptr;
    volatile BottomToTopDataPacket *DataPacket_ptr;
    char c;
    __asm__ volatile ("push CORCON");
    
    
    while(U1STAbits.URXDA) {
    
    if(SerialDataIn == &SerialDataIn_A)   //Ping-Pong
    {
        char_ptr = (char *) &SerialDataIn_B;  //A is valid, so fill B
        DataPacket_ptr = &SerialDataIn_B;
    }
    else
    {
        char_ptr = (char *) &SerialDataIn_A;  //B is valid, so fill A
        DataPacket_ptr = &SerialDataIn_A;
    }
    
    c = U1RXREG;
    switch(state)
    {
        case 0:  
            if (c == 0x55)  //Searching for start of packet and 0x55 received
              {
                char_ptr[state] = c;
                state = 1;  
              }
            break;
        case 1:
            if (c == 0x55)
            {
                char_ptr[state] = c;
                state = 2;  //Got what looks like a preamble.
            }
            else
                state = 0; //False alarm, go back to looking for first preamble byte
            break;
            
        default:  //receive  and store data.
            if(state > 1)
            {
                char_ptr[state] = c;
                state++;
            }
    }
    

    if(state == sizeof(BottomToTopDataPacket)) //Received enough bytes to fill up a packet.
    {


        crc = calcCRC(char_ptr,sizeof(BottomToTopDataPacket)-2);  //Compute CRC   
        if(crc == DataPacket_ptr->crc) //Valid packet received.
        {   
            asm volatile ("disi #0x3FFF");  //Ensure this assignment is atomic
            SerialDataIn = DataPacket_ptr;  //Point at now valid data packet.  This ISR will fill opposite buffer next time as a result. 
            asm volatile ("disi #0x0000");
        }   
        state = 0; //Go back to looking for start of packet.
     
    }
    
    }
    
    _U1RXIF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}