#pragma config(CircuitBoardType, typeCktBoardMega)
#pragma config(Sensor, dgtl22, QuadEncRight,        sensorQuadEncoder)
#pragma config(Sensor, dgtl24, QuadEncLeft,         sensorQuadEncoder)
#pragma config(Sensor, dgtl50, startButton,         sensorDigitalIn)
#pragma config(Motor,  servo_2,         motorRight,    tmotorServoContinuousRotation, openLoop, IOPins, dgtl2, None)
#pragma config(Motor,  servo_3,         motorLeft,     tmotorServoContinuousRotation, openLoop, IOPins, dgtl3, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "New_Utils.h"
#include "SerLCD_Utils.h"

//Prototype My Driving SubTask
task moveForwardStraight();

//Define my RFID Tag String
const string cardA = "2C00AC6FDD";

task main()
{
	bMotorReflected[motorLeft] = true;

	string firstString = "";
	string EncValues = "";

	//Initialize Sparkfun SerLCD 2x16 Module
	configureSerialPort(uartOne, uartUserControl);
	setBaudRate(uartOne, baudRate9600);

	//Intialize Parallax RFID Sensor
	configureSerialPort(uartTwo, uartUserControl);
	setBaudRate(uartTwo, baudRate2400);

	//Initialize Xbee Module
	configureSerialPort(uartThree, uartUserControl);
	setBaudRate(uartThree, baudRate9600);

	// Clear LCD
	SerLCD_ClearScreen(uartOne);

	// Prompt User to Scan Tag
	parseStringToSerial("Press Button 50 ", uartOne);
	parseStringToSerial("To Start Program", uartOne);

	// Wait Until User Presses a Button
	while(SensorValue[dgtl50] == 1);

	// Start Running the Robot
	SerLCD_ClearScreen(uartOne);
	parseStringToSerial("Looking for RFID", uartOne);
	StartTask(moveForwardStraight);

	// Grab an RFID String - Pass String by Reference, then what UART it's on
	firstString = "";
	getRFIDTag(firstString, dgtl28, uartTwo);

	// We've got a card - let's check the data
	if(firstString == cardA)
	{
		//Found the right card, tell user
		SerLCD_ClearScreen(uartOne);
		parseStringToSerial("Found Card A", uartOne);
		//Package up encoder values into a string to send to other users
		sprintf(EncValues, "A/%d/%d", SensorValue[QuadEncRight], SensorValue[QuadEncLeft]);
		//Broadcast Data via Xbee
		parseStringToSerial(EncValues, uartThree);
	}
	else
	{
		//Found a Bad Tag, Inform User
		SerLCD_ClearScreen(uartOne);
		parseStringToSerial("Incorrect Tag", uartOne);
	}

	//Infinite Loop to Let moveForwardStraight to finish
	while(true)
	{
		wait1Msec(25);
	}
}

task moveForwardStraight()
{
	// Clear Encoders
	SensorValue[QuadEncLeft] = 0;
	SensorValue[QuadEncRight] = 0;
	int DistToTravel = 2500;

	//While Loop To Run Until I hit my target distance
	while(SensorValue[QuadEncLeft] < DistToTravel)
	{
		//Auto Straightening Code
		if(SensorValue[QuadEncLeft] > SensorValue[QuadEncRight])
		{
			motor[motorLeft] = 25;
			motor[motorRight] = 35;
		}
		else if(SensorValue[QuadEncLeft] < SensorValue[QuadEncRight])
		{
			motor[motorLeft] = 35;
			motor[motorRight] = 25;
		}
		else
		{
			motor[motorLeft] = 35;
			motor[motorRight] = 35;
		}
	}
	//End of Task, Shut Down Motors
	motor[motorLeft] = 0;
	motor[motorRight] = 0;
}