// #define TestReceive

#include <stdio.h>
#define _LEGACY_HEADERS
#include <htc.h>
#include "BITDEFS.h"
#include "SCIMessage_H.h"
#include "TimerPIC.h"
#include "Communication.h"
#include "Messages.h"
#include "TimerNumber.h"

#define SetBaudRate 0x81


// Define Numerical Constants
#define   TRUE            1
#define   FALSE           0

__CONFIG(UNPROTECT & WDTDIS & PWRTEN & HS);

/*---------------------------- Module Types -----------------------------*/
/*-------------------------- Module Variables ---------------------------*/
static unsigned char messageArray[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
static unsigned char checkSumError;
static ReceiveMessageState State;
static unsigned char MSBState;
static unsigned char LSBState;
static unsigned char ChecksumState;
static unsigned char DataState;
static unsigned char StartByteReceived;
static unsigned char StartState;
static unsigned char ReceiveISR;
static unsigned char byteCounter;
static unsigned char TimeOutFlag;
static unsigned char NewMessageFlag;
static unsigned char Packet[17];
static unsigned char Data[10];
static unsigned char DataLength;
static unsigned char SendCounter;
static unsigned char k = 0;
static unsigned char NewMessageReceived;
static unsigned char TransmitFlag = 0;

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


unsigned char CheckNewMessage(void) {
	// If NewMessageReceived Flag is Set
	if (NewMessageReceived == 1) {
		// Clear NewMessageReceived Flag
		NewMessageReceived = 0;
		// Return 1
		return 1;
	}
	// Otherwise, No Message was Received
	else
		// Return 0
		return 0;

}

unsigned char CheckTransmit(void){
	// If TransmitFlag is set
	if (TransmitFlag == 1) {
		// Reset Transmit Flag
		TransmitFlag = 0;
		// Return 1
		return 1;
	}
	// Otherwise
	else
		// Return 0
		return 0;
}

unsigned char ReturnDataSize(void) {

	return byteCounter;
}

unsigned char QueryMessage(int index){
	return messageArray[index];
}


void InitComm(void) {
unsigned char temp = 1;

// TXSTA Register
TXSTA = 0;
SYNC = 0;		// Enable asynchronous serial transmission port
TXEN = 1;		// Enable Transmissions
BRGH = 1;		// Enable High Data Rate
TX9  = 0;		// 8-bit Transmit

// RCSTA Register
RCSTA = 0;
SPEN = 1; 		// Enable asynchronous serial reception port
CREN = 1;		// Enable Receptions
RX9 = 0; 		// 8-bit receive
ADDEN = 0;		// Disable Address Detection

// SPBRGH = 0;
SPBRG = SetBaudRate; // Baud Rate = 9600 bps (0x81)

temp = RCREG;

// Set all state flags and Counters to 0

DataState = 0;
MSBState = 0;
LSBState = 0;
ChecksumState = 0;
StartByteReceived = 0;
StartState = 0;
ReceiveISR = 0;
TimeOutFlag = 0;
byteCounter = 0;
NewMessageReceived = 0;
}

void EnableInterrupt(void) {

PIR1 = 0;
//RCIE = 1;
PEIE = 0;
GIE = 1;
}

void ReceiveMessageSM(void) {
	static ReceiveMessageState currentState = WAITING_FOR_START_BYTE;
	static unsigned char checkSum;
	static unsigned char messageLength;

	ReceiveMessageState nextState;
	nextState = currentState;

	switch(currentState) {
		// If current State is WAITING_FOR_START_BYTE
		case WAITING_FOR_START_BYTE:
			// If Byte Read is 7E
			if (RCREG == 0x7E){
				StartByteReceived = 1;
				// Message Received, Wait for the MSB
				nextState = WAITING_FOR_MSB;
				// Start Timeout Timer
				TimerPIC_InitTimer(XBEETIMEOUT_TIMER, XBEETIMEOUT);
			}
			// Otherwise do nothing, and keep waiting for start byte
		break;

		// If currentState is WAITING_FOR_MSB
		case WAITING_FOR_MSB:
			MSBState = 1;
			// Timeout Timer has Timed Out
			if(TimerPIC_IsTimerExpired(XBEETIMEOUT_TIMER) == TimerPIC_EXPIRED){
				// Return to WAITING_FOR_START_BYTE State
				nextState = WAITING_FOR_START_BYTE;
			}
			else if(RCIF = 1){ // If we received a byte
				if(RCREG == 0x00){ // If we received a 0
					// Start Timeout Timer
				TimerPIC_InitTimer(XBEETIMEOUT_TIMER, XBEETIMEOUT);
				// Wait for the LSB
				nextState = WAITING_FOR_LSB;
				// Otherwise message was corrupted
				} else{
				// return to WAITING_FOR_START_BYTE State
				nextState = WAITING_FOR_START_BYTE;
				}
			}
		break;

		// If currentState is WAITING_FOR_LSB
		case WAITING_FOR_LSB:
			LSBState = 1;
			// Check if Time Out Occured
			if(TimerPIC_IsTimerExpired(XBEETIMEOUT_TIMER) == TimerPIC_EXPIRED){
				nextState = WAITING_FOR_START_BYTE;
			}
			else if (RCIF = 1){ // If we received a byte
				// Set Next State to WAITING_FOR_DATA
				nextState = WAITING_FOR_DATA;
				// Restart Timeout Timer
				TimerPIC_InitTimer(XBEETIMEOUT_TIMER, XBEETIMEOUT);
				// Initialize the CheckSum and Save the Size of Message
				checkSum = 0;
				messageLength = RCREG;
				// Initialize the byteCounter
				byteCounter = 0;
			}
		break;

		// If currentState is WAITING_FOR_DATA
		case WAITING_FOR_DATA:
			DataState = 1;
			// If Timeout Timer has Timed Out
			if(TimerPIC_IsTimerExpired(XBEETIMEOUT_TIMER) == TimerPIC_EXPIRED){
				// Return to WAITING_FOR_START_BYTE State
				nextState = WAITING_FOR_START_BYTE;
			}
			// If message was received
			else if(RCIF = 1){
				// Restart Timeout Timer
				TimerPIC_InitTimer(XBEETIMEOUT_TIMER, XBEETIMEOUT);
				if (messageLength == 0)
					nextState = WAITING_FOR_CHECKSUM;
				else{
					// Set nextState to WAITING_FOR_DATA
					nextState = WAITING_FOR_DATA;
					// Decrement messageLength
					messageLength--;
					// Add the ReceivedMessage to the running checksum Counter
					checkSum += RCREG;
					// Store the ReceivedMessage into the messageArray
					messageArray[byteCounter] = RCREG;
					// Increment the byteCounter
					byteCounter++;
				}
			}
		break;

		// If currentState is WAITING_FOR_CHECKSUM
		case WAITING_FOR_CHECKSUM:
			ChecksumState = 1;
			// Check If Timeout Occured
			if(TimerPIC_IsTimerExpired(XBEETIMEOUT_TIMER) == TimerPIC_EXPIRED){
				// Return to WAITING_FOR_START_BYTE State
				nextState = WAITING_FOR_START_BYTE;
			}
			// If message was received
			else if (RCIF = 1){
				// nextState is WAITING_FOR_START_BYTE
				nextState = WAITING_FOR_START_BYTE;
				// If 0xFF - Checksum is equal to the received message
				if (RCREG == (0xFF - checkSum)) {
					// Good CheckSum Received, set checksumError flag to 0
					checkSumError = 0;
					// Set NewMessagedReceived Flag to 1
					NewMessageReceived = 1;
				}
				// Otherwise
				else
					// Message was bad, set checkSumError flag to 1
					checkSumError = 1;

			}
		break;

	}

	currentState = nextState;
	State = nextState;
}

// This function puts together a packet to send
// Input Bytes: To Address MSB/LSB, Length, Options, Command Type, and Data Array
void CreatePacket(unsigned char Data_Length, unsigned char MessageAddressMSB, unsigned char MessageAddressLSB, unsigned char Options, unsigned char CommandType, unsigned char DataToSend[]){

    unsigned char i,Checksum;


    Packet[0] = START_MESSAGE;
    Packet[1] = 0x00;                   // Length High Byte
    Packet[2] = Data_Length + 6;         // Length Low Byte-complete message length includes all bytes except checksum
    Packet[3] = API_TRANSMIT;
    Packet[4] = 0x01;                    // Frame ID
    Packet[5] = MessageAddressMSB;
    Packet[6] = MessageAddressLSB;
    Packet[7] = Options;
    Packet[8] = CommandType;

    // Initialize Checksum byte to the sum of bytes 3 to 8
    Checksum = API_TRANSMIT + 0x01 + MessageAddressMSB + MessageAddressLSB + Options + CommandType;

    // For the amount of bytes in the data message
    for(i=0; i<Data_Length; i++){
      	// Store the appropriate bytes into packet
        Packet[i+9] = DataToSend[i];
        // Add the byte to the running checksum counter
        Checksum += DataToSend[i];
    }
	// After the end of the Packet Data, the Checksum byte is 0xFF - Running Checksum Counter
    Packet[Data_Length+9] = 0xFF - Checksum;
    // Save the Data_Length
    DataLength = Data_Length;
    // Set the NewMessageFlag HI
    NewMessageFlag = TRUE;
}

void TransmitSM(void){

    static TransmitState_t CurrentState = WAITING_FOR_PACKET;
    static unsigned char Counter;

    switch(CurrentState){
        // If CurrentState is WAITING_FOR_PACKET
        case WAITING_FOR_PACKET:

		// If transmit Timer has expired
		if(TimerPIC_IsTimerExpired(SEND_TIMER) == TimerPIC_EXPIRED){
			// Turn off Send Debug LED
			RD0 = 0;
			// If NewMessageFlag is True
			if(NewMessageFlag == TRUE){
				// Clear NewMessageFlag
            	NewMessageFlag = FALSE;
            	// Reset Counter to 0
                Counter = 0;
                // Set CurrentState to WAITING_FOR_TRANSMIT
                CurrentState = WAITING_FOR_TRANSMIT;
                // Reinitialize the SEND_LED_TIMEOUT
				TimerPIC_InitTimer(SEND_LED_TIMER, SEND_LED_TIMEOUT);
				// Turn on Send Debug LED
				RD0 = 1;
            }
			TimerPIC_InitTimer(SEND_TIMER, SENDTIMEOUT);
		}
        break;

		// If CurrentState is WAITING_FOR_TRANSMIT
        case WAITING_FOR_TRANSMIT:
         	// If Counter is less than Data Length + 10
            if (Counter < (DataLength+9) + 1){
	            // If Transmission is Ready
                if (TXIF == 1){
          	        // Set the transmit register to the current Packet Byte
          	        TXREG = Packet[Counter];
          	        // Increment the Counter
          	        Counter++;
          	        // Set SendCounter to Counter
					SendCounter = Counter;
                }
            }
            // Otherwise
            else{
	            // If Transmission is done sending
				if (TXIF == 1){
					// Set Transmit Flag to 1
					TransmitFlag = 1;
					// Set Next State to WAITING_FOR_PACKET
	                CurrentState = WAITING_FOR_PACKET;
				}
			}
        break;
    }
}


Message_t ProcessMSG(void){
	Message_t ReceivedMessage = NoMessage;

	// If API Byte was API_RECEIVE
	if (messageArray[API] == API_RECEIVE ){
		// If Receive Option is 0
		if (messageArray[RECEIVE_OPTION] == 0){ // Message was Directed
			// If Command Type is a Atoll Reply
			if (messageArray[COMMAND_TYPE] == REQ_CAPTURE_REPLY){ // Command Type was an Atoll Reply
				// If Message was ATOLL_SUCCESS
				if (messageArray[SUCCESS_FAIL] == ATOLL_SUCCESS)
					// Set ReceivedMessage to AtollSuccess
					ReceivedMessage = AtollSuccess;
				// Otherwise if Message was ATOLL_FAILURE
				else if (messageArray[SUCCESS_FAIL] == ATOLL_FAILURE)
					// Set ReceivedMessage to AtollFailure
					ReceivedMessage = AtollFailure;
			}

			// if Command Type was FIND_TEAM
			if (messageArray[COMMAND_TYPE] == FIND_TEAM)
					// Set Message to TeammateACK
					ReceivedMessage = TeammateACK;
			// else if Command Type was COMMAND_C2B
			else if (messageArray[COMMAND_TYPE] == COMMAND_C2B) // Command Type was an Atoll Reply
				// Set ReceivedMessage to DriveCommand
				ReceivedMessage =  DriveCommand;
			}

		else{ // Message was Broadcast
			// If Message Command was BROADCAST_CAPTURE
			if (messageArray[COMMAND_TYPE] == BROADCAST_CAPTURE)
				// Set Received Message to AtollCaptured
				ReceivedMessage = AtollCaptured;
			// Otherwise if Command was FIND_TEAM
			else if (messageArray[COMMAND_TYPE] == FIND_TEAM)
				// Set Received Message to ColorBroadcast
				ReceivedMessage = ColorBroadcast;
		}

	// Otherwise Message was a Transmit Status
	}else {
			if(messageArray[TRANSMIT_STATUS] == 0) // If message was a succesful reception
				// Set Received message to ModemAck
				ReceivedMessage = ModemACK;
	}

	// Return the ReceivedMessage type
	return ReceivedMessage;
}

void DebugLED(void){
	if(TimerPIC_IsTimerExpired(SEND_LED_TIMER) == TimerPIC_EXPIRED){
		//If Timer Expired, Turn off LED
		RD0 = 0;
	}
}
#ifdef TestReceive
void interrupt ISR(void){
	if (TMR0IE && TMR0IF)
			TimerPIC_ISR();
ReceiveISR = 1;
}

void main (void){
int i;
char checksum = 0;
int index;

// Set Input (TRISA = 1)/ Set Output (TRISA = 0)
TRISA0 = 0;
TRISA1 = 0;

PCFG2 = 1;
TRISB0 = 0;
RB0 = 1;
// Set to Digital (ANSEL = 0) /Analog (ANSEL = 1)
// ANSEL = 0;
// ANSELH = 0;

InitComm();
TimerPIC_Init();
EnableInterrupt();

TimerPIC_InitTimer(SEND_TIMER, SENDTIMEOUT);

Data[0] = 0x01;
Data[1] = 0x02;
Data[2] = 0x03;
Data[3] = 0x04;
Data[4] = 0x04;
Data[5] = 0x05;
Data[6] = 0x06;


// 7E, 00, 07, 01, 01, 21, 8E, 00, CD, DF, A2
while(1){
	TransmitSM();
	ReceiveMessageSM();
	// ProcessMessage();
	CreatePacket(6, CVC_ADDRESS_MSB, CVC_ADDRESS_LSB,DIRECT_MESSAGE, COMMAND_C2B, Data);
	// if (CheckNewMessage() == 1 && ProcessMSG() == DriveCommand) {

}

}

#endif