#include "New_Utils.h"

task main()
{	// Setup XBee RS-485 Adapter
	nxtEnableHSPort();       //Enable High Speed Port #4
	nxtSetHSBaudRate(9600);  //Xbee Default Speed
	nxtHS_Mode = hsRawMode;  //Set to Raw Mode (vs. Master/Slave Mode)

	//Initalize Variables
	short failCount;
	byte incomingData, flushBuffer;
	string rvcData = "";
	int rvcEnc1, rvcEnc2;

	//Flush NXT Internal Buffer
	while(nxtGetAvailHSBytes())
	{
		//Read in any extra bytes that might still be in the buffer
		nxtReadRawHS(flushBuffer, 1);
	}

	// Wait for incoming bytes
	while(!nxtGetAvailHSBytes());

	// Got some bytes, let's process them
	while(failCount < 5)
	{
		//Check to see if we have any data coming in
		if(nxtGetAvailHSBytes())
		{
			//Ask for the data, save to "incomingData"
			nxtReadRawHS(incomingData, 1);

			//Is the data actual data or garbage characters?
			if(isRealChar((char)incomingData))
			{
				//If real data, add to my string.
				rvcData += (char)incomingData;
			}
		}
		//No Data, Wait a little before trying again
		else
		{
			//Number of 'bad' bytes - Once this hits 5, we move on.
			failCount++;
			//Wait 25ms before checking again.
			wait1Msec(25);
		}
	}

	//Convert Gathered Data into Useful
	rvcEnc1 = LongDelStrWithParms(rvcData, '/', 2);
	rvcEnc2 = LongDelStrWithParms(rvcData, '/', 3);

	//Use Motor Encoder Targets to Drive to where the RFID Tag is
	nMotorEncoder[motorB] = 0;
	nMotorEncoder[motorC] = 0;

	//Multiply by 1.25 because of wheel size differences
	nMotorEncoderTarget[motorB] = rvcEnc1 * 1.25;
	nMotorEncoderTarget[motorC] = rvcEnc2 * 1.25;

	motor[motorB] = 50;
	motor[motorC] = 50;

	wait1Msec(15000); //Time for movement to complete
	nxtDisableHSPort();  //Disable HS Port #4
}