#include <htc.h>
#include "pic16f690.h"
#include <stdio.h>
#include "bitdefs.h"
#include "ACVXbeePicUART.h"

__CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS);

/*----------------- Module Definitions ------------------*/
#define Timer0_200ms 16
#define Timer0_130ms 10

/*------------------ Module Functions -------------------*/
static void UpdateTimer0(void);
static void TransmitMachine(void);
static void ReceiveMachine(void);

/*------------------ Module Variables -------------------*/
//Xbee Transmission Variables
static TransmitState_t TransmitState = Transmit_Idle;

static unsigned char XbeeMessageReadyFlag = 0;
static unsigned char XbeeMessageTimer = 0;
static unsigned char XbeeMessageTimerFlag = 1;

static unsigned char XbeeMessage[20];
static unsigned char XbeeMessageLength = 0;
static unsigned char XbeeMessageIndex = 0;
static unsigned char XbeeMessageChecksum;

static unsigned char MotorMessageReadyFlag = 0;
static unsigned char MotorMessage[5];
static unsigned char MotorMessageLength = 4;
static unsigned char MotorMessageIndex = 0;

//Xbee Receive Variables
static ReceiveState_t ReceiveState = Receive_Idle;

static unsigned char ReceiveMessageFlag = 0;
static unsigned char ReceiveMessageTimer = 0;
static unsigned char ReceiveMessageTimerFlag = 0;

static unsigned char ReceiveMessage[20];
static unsigned char ReceiveMessageLength = 0;
static unsigned char ReceiveMessageIndex = 0;
static unsigned char ReceiveMessageChecksum;

//SPI Receive Variables
static SPIState_t SPIState = SPI_Idle;

static unsigned char SPIMessageFlag = 0;
static unsigned char SPIMessageColorFlag = 0;
static unsigned char SPIMessageTimer = 0;
static unsigned char SPIMessageTimerFlag = 0;

static unsigned char SPIMessage[7];
static unsigned char SPIMessageLength = 0;
static unsigned char SPIMessageIndex = 0;

/*-------------------- Module Code ----------------------*/

/*------------------- Init Functions --------------------*/

/**********************************************************
Function:   InitTimer0
Parameters: none
Returns:    none
Description:
    Initializes UART and SPI timeout timer
**********************************************************/
void InitTimer0(void){
    //Timer0 Init (over flows at 76 Hz)
    T0CS = 0; //use Fosc/4
    PSA = 0; //prescale for T0
    PS0 = 1; //Set Prescale to 256.
    PS1 = 1;
    PS2 = 1;
}

/**********************************************************
Function:   InitSPI
Parameters: none
Returns:    none
Description:
    Initializes SPI to Slave
**********************************************************/
void InitSPI(void){
	//TRIS registers for SPI
	TRISB6 = 1; //SCK input
	TRISB4 = 1; //SDI input
	TRISC7 = 0; //SDO output
	TRISC6 = 1; //SS input

    //Configure SPI Status register
    SSPSTAT = 0b00000000;
            //  0xxxxxxx    Required for slave mode
            //  x0xxxxxx    CKE - Data transmitted on rising edge of SCK

    //setup SPI Control register
    SSPCON =  0b00110100;  // setup SPI port as slave
            //  xx1xxxxx    Enables SPI
            //  xxx1xxxx    CKP - clock idles high
            //  xxxx0100    SPI slave with SS enabled
}

/**********************************************************
Function:   InitUART
Parameters: none
Returns:    none
Description:
    Initializes UART to 9600,N,8,1
**********************************************************/
void InitUART(void){
	//TRIS registers for UART
	TRISB5 = 1; //RX input
	TRISB7 = 0; //TX output

    //9600 baud rate:
    SPBRG = 129;
    BRGH = 1;
    //asynch mode:
    SYNC = 0;
    SPEN = 1;
    //enable tx and rx:
    TXEN = 1;
	CREN = 1;
}


/*------------------ Update Functions -------------------*/

/**********************************************************
Function:   UpdateUART
Parameters: none
Returns:    none
Description:
    Global update for UART Xmit and Receive, as well as
    timeout timer.
**********************************************************/
void UpdateUART(void){
    UpdateTimer0();
    TransmitMachine();
    ReceiveMachine();
}

/**********************************************************
Function:   UpdateTimer0
Parameters: none
Returns:    none
Description:
    Increments state timers and sets flag on timeout
**********************************************************/
static void UpdateTimer0(void){
   if (T0IF == 1){  //if timer 0 overflows
        T0IF = 0;   //clear flag

        //Increment Timers
        XbeeMessageTimer++;
        ReceiveMessageTimer++;
        SPIMessageTimer++;

        //200ms transmission timer
        if (XbeeMessageTimer >= Timer0_200ms){
            XbeeMessageTimer = 0;
            XbeeMessageTimerFlag = 1;
        }

        //timeout state timers for receive machines
        if (ReceiveMessageTimer >= Timer0_130ms){
            ReceiveMessageTimer = 0;
            ReceiveMessageTimerFlag = 1;
        }
        if (SPIMessageTimer >= Timer0_130ms){
            SPIMessageTimer = 0;
            SPIMessageTimerFlag = 1;
        }
    }
}


/*-------------------- SPI Functions --------------------*/

/**********************************************************
Function:   UpdateSPI
Parameters: none
Returns:    none
Description:
    SPI State Machine.  Only updates on a new byte over
    SPI, or on a state timeout (i.e., these are the only
    two events).
**********************************************************/
void UpdateSPI(void){
    //On new message, enter state machine
    if (SSPIF == 1){
        SSPIF = 0;  // Clear new message flag
        unsigned char SPINewByte = SSPBUF;
        switch (SPIState){

//State: Waiting for new transmission
            case SPI_Idle:
                //On a new transmission,
                //Clear new message flag:
                SPIMessageFlag = 0;
                //Clear timer and timer flag:
                SPIMessageTimer = 0;
                SPIMessageTimerFlag = 0;
                //Record first byte of message:
                SPIMessage[0] = SPINewByte;
                SPIMessageIndex = 1; //Index of next byte

                //Determine what type of card was scanned
                if (SPINewByte == Start_Color){
                    SPIState = SPI_Data;
                    SPIMessageColorFlag = 1;
                    SPIMessageLength = 4; //length for color message
                }
                if (SPINewByte == Start_Atoll){
                    SPIState = SPI_Data;
                    SPIMessageColorFlag = 0;
                    SPIMessageLength = 7; //length for atoll message
                }
                break;

//State: Processing a transmission
            case SPI_Data:
                //Record new data
                SPIMessage[SPIMessageIndex] = SPINewByte;
                SPIMessageIndex++;

                //If we have reached message length
                if (SPIMessageIndex >= SPIMessageLength){
                    SPIState = SPI_Idle; //return to idle
                    SPIMessageFlag = 1;  //set new message flag
                }

                SPIMessageTimer = 0;
                break;
        }
    }

    //On timeout, reset state machine to idle
    else if (SPIMessageTimerFlag == 1){
        SPIState = SPI_Idle;
        SPIMessageTimerFlag = 0;
    }
}

/**********************************************************
Function:   CheckSPIMessageFlag
Parameters: none
Returns:    unsigned char:  0 if no new message
                            1 if new atoll message
                            2 if new color message
Description:
    Flag checker function for a new message over SPI
**********************************************************/
unsigned char CheckSPIMessageFlag(void){
    unsigned char ReturnVal = SPIMessageFlag * (1 + SPIMessageColorFlag);
    SPIMessageFlag = 0;
    return ReturnVal;
}

/**********************************************************
Function:   ReadSPIMessage
Parameters: unsigned char Index
Returns:    unsigned char: the element of the received
    SPI message indexed by Index
Description:
    Retrieves a byte of a new SPI message.
**********************************************************/
unsigned char ReadSPIMessage(unsigned char Index){
    return SPIMessage[Index];
}


/*------------ UART Transmission Functions --------------*/

/**********************************************************
Function:   TransmitMessage
Parameters: none
Returns:    none
Description:
    Transmit state machine, handles all UART transmissions
    to Xbee and to motor PICs.
**********************************************************/
static void TransmitMachine(void){
	switch (TransmitState){
//State: Waiting to Transmit
		case Transmit_Idle:
    //Event: New Xbee transmission ready, 200ms timer expired
            if ((XbeeMessageTimerFlag == 1) && (XbeeMessageReadyFlag == 1)){
                //Start new Xbee transmission
	    		TransmitState = Transmit_XbeeInProgress;
                XbeeMessageIndex = 0;
        	    XbeeMessageTimerFlag = 0;
            }
    //Event: New Motor transmission ready
            else if (MotorMessageReadyFlag == 1){
                //Start new Motor transmission
    			TransmitState = Transmit_MotorInProgress;
                MotorMessageIndex = 0;
		    }
			break;
//State: Transmitting Xbee Message
		case Transmit_XbeeInProgress:
    //Event: OK to send new byte
            if (TXIF == 1){
                //Send new byte, increment counter
                TXREG = XbeeMessage[XbeeMessageIndex];
                XbeeMessageIndex++;
                //If we have finished sending
                if (XbeeMessageIndex >= XbeeMessageLength){
                    //Return to Idle state
                    TransmitState = Transmit_Idle;
                    XbeeMessageIndex = 0;
                    XbeeMessageReadyFlag = 0;
                }
            }
            break;
//State: Transmitting Motor Message
		case Transmit_MotorInProgress:
    //Event: OK to send new byte
            if (TXIF == 1){
                //Send new byte, increment counter
                TXREG = MotorMessage[MotorMessageIndex];
                MotorMessageIndex++;
                //If we have finished sending
                if (MotorMessageIndex >= MotorMessageLength){
                    //Return to idle state
                    TransmitState = Transmit_Idle;
                    MotorMessageIndex = 0;
                    MotorMessageReadyFlag = 0;
                }
            }
            break;
    }
}

/**********************************************************
Function:   CheckXbeeTransmitOK
Parameters: none
Returns:    unsigned char: 0 if Xbee message already queued
                           1 if OK to add message to queue
Description:
    Checks state of message ready flag (note -- returns
    inverse value to flag value)
**********************************************************/
unsigned char CheckXbeeTransmitOK(void){
    return (XbeeMessageReadyFlag == 0);
}

/**********************************************************
Function:   CheckXbeeTransmitOK
Parameters: none
Returns:    unsigned char: 0 if Motor message already queued
                           1 if OK to add message to queue
Description:
    Checks state of message ready flag (note -- returns
    inverse value to flag value)
**********************************************************/
unsigned char CheckMotorTransmitOK(void){
    return (MotorMessageReadyFlag == 0);
}


/*--------- UART Message Construction Functions ---------*/

/**********************************************************
Function:   StartXbeeTransmission
Parameters: unsigned char FrameID - message Frame ID
            unsigned char Address_High - target address
            unsigned char Address_Low - target address
Returns:    unsigned char: 1 if successfully started
    new message, 0 otherwise
Description:
    Starts a new message to the Xbee, if there is not
    already a message queued up.  Message length and
    checksum are left so that new bytes can be
    subsequently appended.
**********************************************************/
unsigned char StartXbeeTransmission(unsigned char FrameID, unsigned char Address_High, unsigned char Address_Low){
    if (CheckXbeeTransmitOK() == 1){
        XbeeMessageChecksum = Checksum_Total; //Initial checksum
        XbeeMessage[0] = Start_Xbee;
        XbeeMessage[1] = 0x00; //Length High
        XbeeMessage[2] = 0x05; //Length Low, initial
        XbeeMessage[3] = API_Transmit;
            XbeeMessageChecksum -= API_Transmit;
        XbeeMessage[4] = FrameID;
            XbeeMessageChecksum -= FrameID;
        XbeeMessage[5] = Address_High;
            XbeeMessageChecksum -= Address_High;
        XbeeMessage[6] = Address_Low;
            XbeeMessageChecksum -= Address_Low;
        XbeeMessage[7] = Option_None;
            XbeeMessageChecksum -= Option_None;
        XbeeMessageIndex = 8; //Next byte of message
        return 1;
    }else{
        return 0;
    }
}

/**********************************************************
Function:   AddTransmissionData
Parameters: unsigned char ByteToAdd - data for message
Returns:    unsigned char: 1 if successfully added data,
                           0 otherwise
Description:
    Adds data to a message that has been previously
    started but not yet finished.  Updates message length
    and checksum.
**********************************************************/
unsigned char AddTransmissionData(unsigned char ByteToAdd){
    if (CheckXbeeTransmitOK() == 1){
        XbeeMessage[XbeeMessageIndex] = ByteToAdd;  //add to message
        XbeeMessage[2]++;   //increase length byte
        XbeeMessageIndex++; //increment index
        XbeeMessageChecksum -= ByteToAdd; //update checksum
        return 1;
    }else{
        return 0;
    }
}

/**********************************************************
Function:   FinishXbeeTransmission
Parameters: none
Returns:    unsigned char: 1 if successfully completed,
                           0 otherwise
Description:
    Adds checksum byte to end of message, and marks message
    as ready to be sent.
**********************************************************/
unsigned char FinishXbeeTransmission(void){
    if (CheckXbeeTransmitOK() == 1){
        //Add checksum byte:
        XbeeMessage[XbeeMessageIndex] = XbeeMessageChecksum;
        XbeeMessageIndex++;
        XbeeMessageLength = XbeeMessageIndex; //length of message
        XbeeMessageReadyFlag = 1;   //ready for sending
        return 1;
    }else{
        return 0;
    }
}

/**********************************************************
Function:   CreateMotorTransmission
Parameters: unsigned char ServosByte
            unsigned char Motor0Byte
            unsigned char Motor1Byte
Returns:    unsigned char: 1 if successfully added to queue,
                           0 otherwise
Description:
    Queues a new message to the Motor PICs, if there is not
    already a message queued up.  Marks message as ready
    to be sent.
**********************************************************/
unsigned char CreateMotorTransmission(unsigned char ServosByte, unsigned char Motor0Byte, unsigned char Motor1Byte){
    if (CheckMotorTransmitOK() == 1){
        //Add data to message
        MotorMessage[0] = Start_Motor;
        MotorMessage[1] = ServosByte;
        MotorMessage[2] = Motor0Byte;
        MotorMessage[3] = Motor1Byte;
        MotorMessageLength = 4; //length of message
        MotorMessageReadyFlag = 1;  //ready for sending
        return 1;
    }else{
        return 0;
    }
}



/*------------- UART Receive Functions ------------------*/

/**********************************************************
Function:   ReceiveMessage
Parameters: none
Returns:    none
Description:
    Receive state machine, handles reception of UART
    messages from Xbee.  Like the SPI state machine, this
    machine only updates on a new received byte or on a
    timeout.
**********************************************************/
static void ReceiveMachine(void){
    //On a new message, enter state machine
    if (RCIF == 1){
        unsigned char ReceiveNewByte = RCREG; //store new byte
        switch (ReceiveState){

//State: Waiting for new message
            case Receive_Idle:
                //If New Byte is 0x7E
                if (ReceiveNewByte == Start_Xbee){
                    //wait for LengthHigh byte
                    ReceiveMessageFlag = 0;
                    ReceiveState = Receive_LengthHigh;
                    ReceiveMessageTimer = 0;
                }
                break;
//State: Waiting for LengthHigh byte
            case Receive_LengthHigh:
                //If New byte is 0x00
                if (ReceiveNewByte == 0x00){
                    //wait for LengthLow byte
                    ReceiveState = Receive_LengthLow;
                    ReceiveMessageTimer = 0;
                }else{
                    //Otherwise reset state machine
                    ReceiveState = Receive_Idle;
                }
                break;
//State: Waiting for LengthLow byte
            case Receive_LengthLow:
                //On new byte, store as message length
                ReceiveMessageLength = ReceiveNewByte;
                //reset to prepare for new message
                ReceiveMessageChecksum = 0; //reset checkusm
                ReceiveMessageIndex = 0;    //reset index
                //Wait for data:
                ReceiveState = Receive_Data;
                ReceiveMessageTimer = 0;
                break;
//State: Waiting for Data bytes
            case Receive_Data:
                //On new byte, store data
                ReceiveMessage[ReceiveMessageIndex] = ReceiveNewByte;
                //update checksum:
                ReceiveMessageChecksum += ReceiveNewByte;
                ReceiveMessageIndex++;  //increment index
                //If received entire message
                if (ReceiveMessageIndex >= ReceiveMessageLength){
                    //wait for checksum
                    ReceiveState = Receive_Checksum;
                }
                ReceiveMessageTimer = 0;
                break;
//State: Waiting for checksum
            case Receive_Checksum:
                //On new byte, update checksum
                ReceiveMessageChecksum += ReceiveNewByte;
                //Check if checksum is valid
                if (ReceiveMessageChecksum == Checksum_Total){
                    //If so, set new message flag
                    ReceiveMessageFlag = 1;
                }
                //Return to Idle state
                ReceiveState = Receive_Idle;
                ReceiveMessageTimer = 0;
                break;
        }
    }
    //If timeout timer expired, reset state machine
    else if (ReceiveMessageTimerFlag == 1){
        ReceiveState = Receive_Idle;
        ReceiveMessageTimerFlag = 0;
    }
}

/**********************************************************
Function:   CheckReceiveMessageFlag
Parameters: none
Returns:    unsigned char:  0 if no new message
                            1 if new message
Description:
    Flag checker function for a new message from Xbee.
    New message could be transmit status or received
    message.
**********************************************************/
unsigned char CheckReceiveMessageFlag(void){
    unsigned char ReturnVal = ReceiveMessageFlag;
    ReceiveMessageFlag = 0;
    return ReturnVal;
}

/**********************************************************
Function:   ReadSPIMessage
Parameters: unsigned char Index
Returns:    unsigned char: the element of the received
    UART message indexed by Index
Description:
    Retrieves a byte of a new message from Xbee.
**********************************************************/
unsigned char ReadXbeeMessage(unsigned char Index){
    return ReceiveMessage[Index];
}