#include <stdio.h>
#include "xc.h"
#include "math.h"
#include "UART.h"
#include "config.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) {
        
#ifdef ALLOW_RESET_BY_RS232
        case 'r':  //Reset processor
        asm("RESET"); //Reset micro-controller.
        break;
#endif     

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

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

        case '1': //Toggle 
            LASER_PWR_ACTIVELOW = ~LASER_PWR_ACTIVELOW;
        break;
        case '2': //Toggle 
            EXTENDER_ACTIVELOW = ~EXTENDER_ACTIVELOW;
        break;
        case '3': //Toggle 
            CAMERA_PWR = ~CAMERA_PWR;
        break; 
        
        case 'z':
            if(RateZeroAdjust > -32766)
               RateZeroAdjust--;
            break;
        case 'x':
            if(RateZeroAdjust < 32766)
               RateZeroAdjust++;
            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 TopToBottomDataPacket *DataPacket_ptr;
    char c;
    __asm__ volatile ("push CORCON");
    

  
    
    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:  //recieve  and store data.
            if(state > 1)
            {
                char_ptr[state] = c;
                state++;
            }
    }
    

    if(state == sizeof(TopToBottomDataPacket)) //Received enough bytes to fill up a packet.
    {
        crc = calcCRC(char_ptr,sizeof(TopToBottomDataPacket)-2);  //Compute CRC   
        if(crc == DataPacket_ptr->crc) //Valid packet received.
        {          
            SerialDataIn = DataPacket_ptr;  //Point at now valid data packet.  This ISR will fill opposite buffer next time as a result.  
        }   
        state = 0; //Go back to looking for start of packet.
    }
    _U1RXIF = 0; //Clear Interrupt
    __asm__ volatile ("pop CORCON");

}