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

__CONFIG(CP_OFF & WDTE_OFF & PWRTE_ON & FOSC_HS);

#define FALSE 0
#define TRUE 1
#define ON 1
#define OFF 0
#define _MS *10/8 //timer overflows every 0.8 ms...
#define TimeoutTime 10_MS
#define ACV_MSB 0x21
#define ACV_LSB 0x86

#define Green 0x01
#define Red 0xFE

#define GreenServo 1
#define RedServo 0

/**************************************************************************************/
//Module types

typedef enum{
	WaitingDelimiter,
	WaitingLengthMSB,
	WaitingLengthLSB,
	WaitingData,
	WaitingCheckSum,
	ErrorInRX
} RXStates_t;

typedef enum{
	WaitingToSend,
	SendingDelimiter,
	SendingLengthMSB,
	SendingLengthLSB,
	SendingAPIID,
	SendingFrameID,
	SendingDestAddMSB,
	SendingDestAddLSB,
	SendingOptByte,
	SendingDataBytes,
	CalculatingCheckSum,
	SendingCheckSum
} XmitStates_t;

typedef enum{
	NoColor,
	GreenTeam,
	RedTeam
} AtollStates_t;

typedef enum{
	TC_Green,
	TC_PendingGreen,
	TC_None,
	TC_PendingRed,
	TC_Red
} TeamColor_t; //For finding team color

typedef enum{
	IO_RunAD,
	IO_CalcPower,
	IO_CalcDirection,
	IO_CalcMapping,
    IO_CalcPowerBoost,
    IO_ResetDriveArray
} CheckIOStates_t; //For CheckIO's

/**************************************************************************************/
//Function prototypes

void Initialization(void);
void CheckTimer(void);
void XmitStateMachine(void);
void CheckIO(void);
void RXStateMachine(void);
void Response_RXGoodMessage(void);
void UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber);
void SetLEDs(void);
void wait_us(unsigned long microsecs);
signed int SQRT(signed int a);
signed int CalculatePower(signed int y_accel, signed int x_accel);
signed int CalculateDirection(signed int Power, signed int x_accel);
void CalibrationCheck(void);
void RunAD(char Chan0, char Chan1, char Chan2, char Chan3);
void UpdateTeamColor(unsigned char OurTeamColor);

/**************************************************************************************/
//Module Variables

static unsigned char Xmit_DestAddMSB;
static unsigned char Xmit_DestAddLSB;
static unsigned char Xmit_DataLength;

static unsigned char Drive_DataByteArray[10];
static unsigned char Servo_DataByteArray[10] = 0;

static unsigned char TimerOverflowFlag = FALSE;
static unsigned char TimerOverflowFlag2 = FALSE;
static XmitStates_t CurrentXmitState = WaitingToSend;
static unsigned char InitiateXmitIOFlag = FALSE;
static unsigned char InitiateXmitServosFlag = FALSE;
static unsigned int CurrentTimerTick = 0;
static unsigned char IncomingData[15];
static unsigned char IncomingDataLength;

static unsigned char RXGoodMessageFlag = FALSE;
static AtollStates_t Atoll[] = {NoColor,NoColor,NoColor,NoColor,NoColor,NoColor,NoColor,NoColor};
static char ServoStates[]={OFF,OFF,OFF,OFF,OFF,OFF,OFF,OFF};


static unsigned char ServoOnOffByte = 0;  // Byte 1 that I send to servos
static unsigned char ServoColorByte = 0; // Byte 2 that I send to servos
static unsigned char TeamColorByte = 0x88;  // Byte 3 that I send to servos, initialize to no color


static unsigned char UpdateColorsFlag = FALSE;
static signed char Output_LeftMotorSpeed;
static signed char Output_RightMotorSpeed;

static CheckIOStates_t CurrentIOState = IO_RunAD;

/**************************************************************************************/
/* Function: main
 * Usage: main()
 * --------------
 * This initializes and then in an infinite loop looks at the timer (to check overflows),
 * calls the Inputs state machine, transmit state machine, and receive transmission. If
 * a good message was received it calls the appropriate function to handle the message.
 */

void main(void){

    Initialization();

    while(1){
        CheckTimer();

		if((InitiateXmitIOFlag == FALSE) && (InitiateXmitServosFlag == FALSE)){
			CheckIO();
		}

		XmitStateMachine();

        RXStateMachine();

        if(RXGoodMessageFlag == TRUE){
            Response_RXGoodMessage();
        }

    }
}

/**************************************************************************************/
/* Function: Initialization
 * Usage: Initialization()
 * -------------------------------
 * This initializes all input ports, as well as the A/D conversion, EUART, and TIMER0.
 */

void Initialization(void){

	//Clear ANSEL registers, except AN0, AN4, AN5
	ANSEL = 0;
	ANSELH = 0;
	ANS4 = 1; //x-axis
	ANS5 = 1; //y-axis
    ANS0 = 1; // Power boost

    //Output for swap motor input button
    TRISC4 = 1;

	//TRIS registers for EUSART
	TRISB |= BIT5HI;	//RX input
	TRISB &= BIT7LO;	//TX output

	//Inputs for for accelerometer
	TRISC0 = 1; //x-axis
	TRISC1 = 1; //y-axus

    //Debug LEDs
    TRISA1 = 0;
    TRISA2 = 0;

    RA1 = 1;
    RA2 = 1;


//AD Conversion
	//AD conversion Fosc/32
    ADCS0 = 0;
    ADCS1 = 1;
    ADCS2 = 0;
    VCFG = 0; //Use Vdd as voltage reference

	//Select AN4 for now
	CHS0 = 0;
	CHS1 = 0;
	CHS2 = 1;
	CHS3 = 0;

	ADFM = 0; //Left justified
	ADON = 1; //Enable ADC

//EUSART init
    SPBRG = 129;	// For 9.6kBaud
    TXSTA |= BIT2HI; //BRGH;
    //asynch mode:
    //TXSTA &= ~SYNC; // defaults to asynchronous already
    RCSTA |= BIT7HI; //SPEN;
    //enable tx and rx:
    TXSTA |= BIT5HI; //TXEN;
	CREN = 1;

// Initialize Timer0
	T0CS = 0; // Use internal Fosc/4
	PSA = 0;
	PS0 = 1; // All of this is /16... about 0.8ms per overflow
	PS1 = 1;
	PS2 = 0;
	//Timer 0 is default enabled...

//Initialize Servos (scoreboard and team color)
    Servo_DataByteArray[0] = 0xB9;
    Servo_DataByteArray[1] = 0b00000000;
    Servo_DataByteArray[2] = 0b00000000;
    Servo_DataByteArray[3] = 0x88;
}
/**************************************************************************************/
/* Function: XmitStateMachine
 * Usage: XmitStateMachine()
 * ---------------------------
 * This state machine is used to transmit over EUART motor commands to the ACV (through
 * the xbee), as well as transmitting servo states for the servos that indicate the
 * score and team color on our CVC.
 */


void XmitStateMachine(void){
	static unsigned char DelimiterByte = 0x7E;
	static unsigned char LengthMSB = 0x00;
	static unsigned char LengthLSB = 0; //Will be updated later
	static unsigned char APIID = 0x01; //Indicates sending packet of data
	static unsigned char DestAddMSB;
	static unsigned char DestAddLSB;
	static unsigned char OptByte = 0x00; //No options
	static unsigned char DataByteArray[10]; //Chose 10 randomly as an upper bound
	static unsigned char AccumulatingCheckSum = 0;
	static unsigned char DataByteIndex;
	static unsigned char DataLength; //Dictated by "Global" input
	static unsigned char FrameID = 0;
    unsigned char j = 0;


	DestAddMSB = ACV_MSB;
	DestAddLSB = ACV_LSB;
	DataLength = 4; // Anything I have to xmit is only 4 bytes long

	LengthLSB = DataLength + 5; // 5 is for APIID, FrameID, DestAddMSB, DestAddLSB, Options

	//The first data byte is sent during the SendingOptByte state, so at that state
	//we check to see if the current xmit is either sending a motor command or update
	//servos color command. Depending on which we extract the appropriate data byte array
	//to send.
	if(CurrentXmitState == SendingOptByte){
		if(InitiateXmitIOFlag == TRUE){
			for(j=0; j<10;j++){
				DataByteArray[j] = Drive_DataByteArray[j];
			}
		}else if(InitiateXmitServosFlag == TRUE){
			for(j=0; j<10;j++){
		        DataByteArray[j] = Servo_DataByteArray[j];
			}
		}
	}

	//Here begins the actual state machine
	switch(CurrentXmitState){
		case WaitingToSend:
			if(TXIF == 1){
				if(TimerOverflowFlag == TRUE){
					TimerOverflowFlag = FALSE; //Fires every 200ms

					InitiateXmitIOFlag = TRUE;
					TXREG = DelimiterByte;
					CurrentXmitState = SendingDelimiter;
				}else if(TimerOverflowFlag2 == TRUE){
					TimerOverflowFlag2 = FALSE; //Fires between every 200ms flag

					InitiateXmitServosFlag = TRUE;
					CurrentXmitState = SendingOptByte;
				}
			}
			break;
		case SendingDelimiter:
			if(TXIF == 1){
				TXREG = LengthMSB;
				CurrentXmitState = SendingLengthMSB;
			}
			break;
		case SendingLengthMSB:
			if(TXIF == 1){
				TXREG = LengthLSB;
				CurrentXmitState = SendingLengthLSB;
			}
			break;
		case SendingLengthLSB:
			if(TXIF == 1){
				TXREG = APIID;
				CurrentXmitState= SendingAPIID;
                AccumulatingCheckSum = 0;
				AccumulatingCheckSum += APIID; //start the checksum
			}
			break;
		case SendingAPIID:
			if(TXIF == 1){
				TXREG = FrameID; //Don't need to check frame ID since I am always sending
				CurrentXmitState = SendingFrameID;
				AccumulatingCheckSum += FrameID;
			}
			break;
		case SendingFrameID:
			if(TXIF == 1){
				TXREG = DestAddMSB;
				CurrentXmitState = SendingDestAddMSB;
				AccumulatingCheckSum += DestAddMSB;
			}
			break;
		case SendingDestAddMSB:
			if(TXIF == 1){
				TXREG = DestAddLSB;
				CurrentXmitState = SendingDestAddLSB;
				AccumulatingCheckSum += DestAddLSB;
			}
			break;
		case SendingDestAddLSB:
			if(TXIF == 1){
				TXREG = OptByte;
				CurrentXmitState = SendingOptByte;
				AccumulatingCheckSum += OptByte;
			}
			break;
		case SendingOptByte:
			if(TXIF == 1){
				DataByteIndex = 0; // Initialize index
				TXREG = DataByteArray[DataByteIndex];
				CurrentXmitState = SendingDataBytes;
				AccumulatingCheckSum += DataByteArray[0]; //Begin sending data
			}
			break;
		case SendingDataBytes:
			if(TXIF == 1){
				DataByteIndex++; //This works because DataLength starts at 1, while index starts at 0
				if(DataByteIndex >= DataLength){ //if finished sending all data, go to next state
					CurrentXmitState = CalculatingCheckSum;
                    if(InitiateXmitServosFlag == TRUE){
                        InitiateXmitServosFlag = FALSE;
                        CurrentXmitState = WaitingToSend;
                    }
				}else{
					TXREG = DataByteArray[DataByteIndex];
					AccumulatingCheckSum += DataByteArray[DataByteIndex];
				}
			}
			break;
		case CalculatingCheckSum:
			if(TXIF == 1){
				TXREG = 0xFF - AccumulatingCheckSum;
				CurrentXmitState = SendingCheckSum;
				AccumulatingCheckSum = 0;
			}
			break;
		case SendingCheckSum:
			if(TXIF == 1){
				CurrentXmitState = WaitingToSend;

				InitiateXmitIOFlag = FALSE; //Reset both flags no matter what at the end
				InitiateXmitServosFlag = FALSE;
			}
			break;
	}

}

/**************************************************************************************/
/* Function: RXStateMachine
 * Usage: RXStateMachine()
 * ------------------------
 * This state machine is used to receive data from the Xbee. There are timeouts
 * implemented at each state, and if a valid message was received (checksum was correct)
 * then a flag is set to be true to indicate that a good message was received (RXGoodMessageFlag).
 */

void RXStateMachine(void){
	static RXStates_t CurrentRXState = WaitingDelimiter;
	static unsigned char IncomingDataIndex;
	static unsigned int LastTimerTick; //For the timout
	static unsigned char AccumulatingCheckSum;
	unsigned IncomingCheckSum;

	//Here begins actual state machine
	switch(CurrentRXState){
		case WaitingDelimiter:
			if(RCIF == TRUE){
				if(RCREG == 0x7E){
					CurrentRXState = WaitingLengthMSB;
					LastTimerTick = CurrentTimerTick;
				}else{
					CurrentRXState = ErrorInRX;
				}
			}
			break;
		case WaitingLengthMSB:
			if(RCIF == TRUE){
				if(RCREG == 0x00){ //MSB should be 0, we don't have very long data
					CurrentRXState = WaitingLengthLSB;
					LastTimerTick = CurrentTimerTick;
				}else{
					CurrentRXState = ErrorInRX;
				}
 			}else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout
				CurrentRXState = ErrorInRX;
			}
			break;
		case WaitingLengthLSB:
			if(RCIF == TRUE){
				IncomingDataLength = RCREG;
				CurrentRXState = WaitingData;
				LastTimerTick = CurrentTimerTick;
				IncomingDataIndex = 0; // Initialize the index here, where it starts
				AccumulatingCheckSum = 0;
			}else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout
				CurrentRXState = ErrorInRX;
			}
			break;
		case WaitingData:
			if(RCIF == TRUE){
				IncomingData[IncomingDataIndex] = RCREG;
				AccumulatingCheckSum += IncomingData[IncomingDataIndex];
				IncomingDataIndex++;
 				LastTimerTick = CurrentTimerTick;

				if(IncomingDataIndex >= IncomingDataLength){ //If all data has come in, go to next state
					CurrentRXState = WaitingCheckSum;
				}
			}else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout
				CurrentRXState = ErrorInRX;
			}
			break;
		case WaitingCheckSum:
			if(RCIF == TRUE){
				IncomingCheckSum = RCREG;

				if((IncomingCheckSum + AccumulatingCheckSum) != 0xFF){ //Check to see if checksum valid
					CurrentRXState = ErrorInRX;
				}else{
					RXGoodMessageFlag = TRUE; //Trigger good message flag
					CurrentRXState = WaitingDelimiter;
				}
			}else if((CurrentTimerTick - LastTimerTick) >= TimeoutTime){ //timeout
				CurrentRXState = ErrorInRX;
			}
			break;
		case ErrorInRX: //Error just returns you to first state
			CurrentRXState = WaitingDelimiter;
			break;
	}
}

/**************************************************************************************/
/* Function: Response_RXGoodMessage
 * Usage: Response_RXGoodMessage()
 * -------------------------------
 * This response routine extracts the actual command data, then checks to see if it was
 * either a command to change the score servos or the team color servo, and calls the
 * the appropriate function to do one or the other.
 */

void Response_RXGoodMessage(void){
	unsigned char RXCommandDataLength;
	unsigned char RXFrameID;
	unsigned char RXSourceAddMSB;
	unsigned char RXSourceAddLSB;
	unsigned char RXIncomingDataOptions;
	unsigned char RXCommandData[10]; //Arbritrary for now
	unsigned char i;

    RXGoodMessageFlag = FALSE;

	if(IncomingData[0] == 0x81){
		RXCommandDataLength = IncomingDataLength - 5; // 5 = APIID, SourceMSB, SourceLSB, RSSI, Options
		RXSourceAddMSB = IncomingData[1];
		RXSourceAddLSB = IncomingData[2];
		//IncomingData[3]; This is signal strength; ignore
		RXIncomingDataOptions = IncomingData[4];

		for(i = 0; i < RXCommandDataLength; i++){
			RXCommandData[i] = IncomingData[5+i];
		}

		if((RXSourceAddMSB == ACV_MSB) && (RXSourceAddLSB == ACV_LSB) && ((RXIncomingDataOptions & BIT1HI) != BIT1HI)){
			//ACV is sending me teamcolor
			if(RXCommandDataLength == 2){
				if(RXCommandData[0] == 0xEF){
					UpdateTeamColor(RXCommandData[1]);
                    RA1^=1;
				}
			}
			//else it is a broadcast to update atoll colors
		}else if((RXIncomingDataOptions & BIT1HI) == BIT1HI){
			if(RXCommandDataLength == 3){
				if(RXCommandData[0] == 0xAB){ //This means that an Atoll was captured
					UpdateAtollColors(RXCommandData[1],RXCommandData[2]);
				}
			}
		}
	}
}

/**************************************************************************************/
/* Function: UpdateTeamColor
 * Usage: UpdateTeamColor(unsigned char OurTeamColor)
 * --------------------------------------------------
 * This updates the appropriate servo data byte (Servo_DataByteArray[3]) with the input OurTeamColor.
 * It just passes on this input to the servo PIC.
 */

void UpdateTeamColor(unsigned char OurTeamColor){
	TeamColorByte = OurTeamColor;

	if(TeamColorByte == 0x88){ // If white key fob is swiped
		ServoOnOffByte = 0; //Reset the atoll color scores
		ServoColorByte = 0;
	}

	Servo_DataByteArray[0] = 0xB9;
	Servo_DataByteArray[3] = TeamColorByte; //Just pass on the team color byte to servo PIC
	UpdateColorsFlag = TRUE; //Don't use anymore, but there just in case we need it.
}

/**************************************************************************************/
/* Function: UpdateAtollColors
 * Usage: UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber)
 * ----------------------------------------------------------------------------
 * This is called when an atoll is captured using the color of the team that captured
 * as one input and the atoll number that was captured as the other input.
 * It updates the two appropriate bytes associated with this: Servo_DataByteArray[1] and
 * Servo_DataByteArray[2]. The first byte is just an On/Off byte to indicate whether
 * a servo should be on and not on the neutral position. The second byte inciates color,
 * 0 for red and 1 for green.
 */

void UpdateAtollColors(unsigned char TeamColor, unsigned char AtollNumber){

	unsigned char i;

	//Update array of Atoll colors and whether the servo should be on
	if(TeamColor == Green){
		Atoll[AtollNumber-1] = GreenTeam;// -1 because index starts at 0
		ServoStates[AtollNumber-1] = ON;
	}else if(TeamColor == Red){
		Atoll[AtollNumber-1] = RedTeam;
		ServoStates[AtollNumber-1] = ON;
	}else if(TeamColor == NoColor){
		Atoll[AtollNumber-1] = NoColor;
		ServoStates[AtollNumber-1] = OFF;
	}

	ServoColorByte = 0;
	ServoOnOffByte = 0;

	//Takes the two arrays above and extracts it into two single bytes
	for(i = 0; i < 8; i++){
		if(Atoll[7-i] == GreenTeam){
			ServoColorByte = (ServoColorByte*2) + 1;
		}else if(Atoll[7-i] == RedTeam){
			ServoColorByte = (ServoColorByte*2);
		}else if(Atoll[7-i] == NoColor){
			ServoColorByte = (ServoColorByte*2);
		}
	}

	for(i = 0; i < 8; i++){
		if(ServoStates[7-i] == OFF){
			ServoOnOffByte = (ServoOnOffByte*2);
		}else{ //Servo is probably on
			ServoOnOffByte = (ServoOnOffByte*2) + 1;
		}
	}

	Servo_DataByteArray[1] = ServoOnOffByte;
	Servo_DataByteArray[2] = ServoColorByte;

	UpdateColorsFlag = TRUE; //Don't use anymore, but there just in case we need it.

	return;
}



/**************************************************************************************/
/* Function: RunAD
 * Usage: RunAD(char Chan3, char Chan2, char Chan1, char Chan0)
 * -------------------------------------------------------------
 * This just takes the four bits it takes to set a port to run an A/D convresion.
 * It does the necessary minimal blocking to get this process done. After, this function
 * is run, you can check ADRESH for the A/D value.
 */

void RunAD(char Chan3, char Chan2, char Chan1, char Chan0){
    unsigned char i;

	CHS0 = Chan0;
	CHS1 = Chan1;
	CHS2 = Chan2;
	CHS3 = Chan3;

	ADIF = 0;
	for(i = 0; i<50; i++){} //Dead for loop for acquisition ~20us
	GO_DONE = 1; //Starts Conversion process
	while(ADIF == 0){}

    return;
}

/**************************************************************************************/
/* Function: CheckIO
 * Usage: CheckIO()
 * ------------------
 * This is actually a state machine because some parts of the code take so long to run
 * that it was appropriate to do so. It checks the accelerometer outputs to do appropriate
 * left/right motor mapping. It checks the FSR output to see how much power boost there
 * should be. And it checks the RC4 button to see if we want to swap the motors.
 */

void CheckIO(void){
	static signed int X_ADConversion;
	static signed int Y_ADConversion;
    static signed int X_Coordinate;
    static signed int Y_Coordinate;
	static signed int Direction;
	static signed int Power;
	static signed int absDirection;
	static signed int absLeftMotorSpeed;
	static signed int absRightMotorSpeed;
	static signed int LeftMotorSpeed;
	static signed int RightMotorSpeed;
    static signed int ChangeDirections;

//Power boost variables
    static signed int FSR_ADConversion;

//Data bytes to transmit
	static unsigned char DataByte1 = 0;
	static unsigned char DataByte2 = 0;
	static unsigned char DataByte3 = 0;

	switch(CurrentIOState){
		case IO_RunAD:
            //Select AN4

                RunAD(0,1,0,0);
            	X_ADConversion = ADRESH;

            //Select AN5
                RunAD(0,1,0,1);
            	Y_ADConversion = ADRESH;

            //Select AN0 for Left Power Boost
                RunAD(0,0,0,0);
            	FSR_ADConversion = ADRESH;

            //Check change directions button (it's really a flip in that left becomes right, and vice versa)
                ChangeDirections = RC4;

            CurrentIOState = IO_CalcPower;
			break;

        case IO_CalcPower:
            //Make x and y go from -128 to 127
            	X_Coordinate = (X_ADConversion - 1) - 127;
            	Y_Coordinate = (Y_ADConversion - 1) - 127;

                Power = CalculatePower(Y_Coordinate, X_Coordinate);

            CurrentIOState = IO_CalcDirection;
			break;

        case IO_CalcDirection:
                Direction = CalculateDirection(Power, X_Coordinate);

            CurrentIOState = IO_CalcMapping;
			break;

        case IO_CalcMapping:

			//Does our motor mapping as well as makes them absolutes
        	absDirection = (Direction >= 0) ? Direction : -Direction;
        	LeftMotorSpeed  = (((Power*Direction) >= 0) ? Power : ((127 - 2*absDirection)*Power/127));
        	RightMotorSpeed = (((Power*Direction) <= 0) ? Power : ((127 - 2*absDirection)*Power/127));
        	absLeftMotorSpeed = ((LeftMotorSpeed >= 0) ? LeftMotorSpeed : -LeftMotorSpeed);
        	absRightMotorSpeed = ((RightMotorSpeed >= 0) ? RightMotorSpeed : -RightMotorSpeed);

			//Assigns appropriate bits for direction, 0 means reverse, 1 means forward
            DataByte1 = 0;
        	if(RightMotorSpeed > 0){
                if(ChangeDirections == OFF){
        		    DataByte1 |= BIT1HI;
                }else{
                    DataByte1 |= BIT0HI;
                }

        	}

        	if(LeftMotorSpeed > 0){
                if(ChangeDirections == OFF){
        	        DataByte1 |= BIT0HI;
                }else{
                    DataByte1 |= BIT1HI;
                }
        	}


            CurrentIOState = IO_CalcPowerBoost;
			break;

        case IO_CalcPowerBoost:
            //Power Boost functionality, scales appropriately the power.

                if((FSR_ADConversion >= 26) && (FSR_ADConversion <= 164)){
                    absLeftMotorSpeed = (absLeftMotorSpeed*(FSR_ADConversion + 36))/200;
                    absRightMotorSpeed = (absRightMotorSpeed*(FSR_ADConversion + 36))/200;
                }else if(FSR_ADConversion > 164){
                    absLeftMotorSpeed = (absLeftMotorSpeed);
                    absRightMotorSpeed = (absRightMotorSpeed);
                }else if(FSR_ADConversion < 26){
                    absLeftMotorSpeed = 0;
                    absRightMotorSpeed = 0;
                }

            CurrentIOState = IO_ResetDriveArray;
			break;

        case IO_ResetDriveArray:

            if(ChangeDirections == OFF){
                DataByte1 |= BIT7HI; // For RFID Spoof activation
            }
        	Drive_DataByteArray[0] = 0xCD;
        	Drive_DataByteArray[1] = DataByte1;

			//Flip the motors if button was pressed
            if(ChangeDirections == OFF){
        	    Drive_DataByteArray[2] = 2*((unsigned char)absLeftMotorSpeed);
        	    Drive_DataByteArray[3] = 2*((unsigned char)absRightMotorSpeed);
            }else{
        	    Drive_DataByteArray[3] = 2*((unsigned char)absLeftMotorSpeed);
        	    Drive_DataByteArray[2] = 2*((unsigned char)absRightMotorSpeed);
            }

            CurrentIOState = IO_RunAD;
            break;
    }
}

/**************************************************************************************/
/* Function: CheckTimer
 * Usage: CheckTimer()
 * --------------------
 * This timer updates for two flags. TimerOverflowFlag is one that fires every 200ms.
 * TimerOverflowFlag2 fires between every one of those 200ms flags.
 */

// This is a timer check that amounts to 5Hz using overflows

void CheckTimer(void){
	static unsigned char i=0;
	if(T0IF == 1){
		i++;
		CurrentTimerTick++;
		T0IF = 0;

        if(i == 120){
            TimerOverflowFlag2 = TRUE;
		}

		if(i>=245){
			i=0;
			TimerOverflowFlag = TRUE;
		}
	}
}

/**************************************************************************************/
/* Function: CalculatePower
 * Usage: CalculatePower(signed int y_accel, signed int x_accel)
 * -------------------------------------------------------------
 * We calculate power by squaring x and y, and taking the square root. This function just
 * calls the square root function.
 */

signed int CalculatePower(signed int y_accel, signed int x_accel){
    signed int R_squared;
    signed int Power;
    signed int abs_x_accel_temp;
    signed int abs_y_accel_temp;
    signed int abs_x_accel;
    signed int abs_y_accel;

    abs_x_accel_temp = ((x_accel > 0) ? x_accel : - x_accel);
    abs_y_accel_temp = ((y_accel > 0) ? y_accel : - y_accel);

    abs_x_accel = abs_x_accel_temp;
    abs_y_accel = abs_y_accel_temp;

    R_squared = (abs_x_accel*abs_x_accel) + (abs_y_accel*abs_y_accel);

    Power = SQRT(R_squared);

    //make sure power >= x_accel


    Power = ((Power > abs_x_accel) ? Power : abs_x_accel);

    //Check to see if one of accel directions is negative, if so make power negative
    if((y_accel < 0)){
            Power = -Power;
        }


    return Power;
}

/**************************************************************************************/
/* Function: SQRT
 * Usage: SQRT(signed int a)
 * -------------------------
 * Calculates a square root with approximation.
 */

signed int SQRT(signed int a){
    signed int SQRT_Result;
    if(a <= 1000){
        SQRT_Result = (a*23)/1000;
    }

    else if( (a > 1000) && (a <= 17000) ){
        SQRT_Result = 75 + (a/300) - (46875)/(((a/225) + 25) * ((a/225) + 25));
    }

    else if( (a > 17000) ){
        SQRT_Result = 127;
    }

    return(SQRT_Result);
}
/**************************************************************************************/
/* Function: CalculateDirection
 * Usage: CalculateDirection(signed int Power, signed int x_accel)
 * ---------------------------------------------------------------
 * Takes newly found power and x value, and just takes their quotient (x/power) to get
 * an approximate direction.
 */

signed int CalculateDirection(signed int Power, signed int x_accel){
    signed int Direction;

    Power = ((Power > 0) ? Power : - Power);

    if(Power == 0){
        Direction = 0;
    }else{
        Direction = ((127*x_accel)/Power);
    }
    return(Direction);
}

/**************************************************************************************/