#pragma config(Sensor, S2,     Sonar,          sensorSONAR)
#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Motor,  motorB,          RightMotor,    tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          LeftMotor,     tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//This is the include for XBee
#include "XbeeTools.h"

task main()
{
  //This initializes the XBee
  InitRS485();
  int count=0;
  string data="";

  wait1Msec(5000);

  //While the sonar does not see an obstacle
  while(SensorValue(Sonar) >=26)
    {
       //If it sees black...
       if (SensorValue(Light)<45)
       {
         //...add one to the counter and move off the black line
         count=count+1;
         motor[RightMotor]= 50;
         motor[LeftMotor]= 50;
         wait1Msec(200);
       }
       //Otherwise, keep moving forward
       motor[RightMotor]=50;
       motor[LeftMotor]= 50;

       //Display the count at all times.
       StringFormat(data, "%d", count);
       nxtDisplayCenteredTextLine(4, data);
    }

    motor[LeftMotor]=0;
    motor[RightMotor]=0;

    //Send string to NXT B
    SendString(data);
    ReceiveString(data);

   //If the received data is "Blue", turn to the blue goal
   if (data=="Blue")
        {
          nMotorEncoder[LeftMotor]=0;
          nMotorEncoder[RightMotor]=0;

          nMotorEncoderTarget[LeftMotor]=160;
          nMotorEncoderTarget[RightMotor]=-160;

          motor[LeftMotor]=50;
          motor[RightMotor]=-50;

          while((nMotorRunState[LeftMotor] != runStateIdle) && (nMotorRunState[RightMotor] != runStateIdle))
          {}

          motor[LeftMotor]=0;
          motor[RightMotor]=0;

        }

     //If the received data is "Red", turn to the red goal
     if (data=="Red")
        {
          nMotorEncoder[LeftMotor]=0;
          nMotorEncoder[RightMotor]=0;

          nMotorEncoderTarget[LeftMotor]=-160;
          nMotorEncoderTarget[RightMotor]=160;

          motor[LeftMotor]=-50;
          motor[RightMotor]=50;

          while((nMotorRunState[LeftMotor] != runStateIdle) && (nMotorRunState[RightMotor] != runStateIdle))
          {}

          motor[LeftMotor]=0;
          motor[RightMotor]=0;

        }

       //Move forward 2 squares: ((2*9.5)/7.0685775)*360
       nMotorEncoder[LeftMotor]=0;
       while (nMotorEncoder[LeftMotor]<968)
        {
          //Move forwards
          motor[RightMotor]=30;
          motor[LeftMotor]=30;
        }
        //Stop the motors
        motor[LeftMotor]=0;
        motor[RightMotor]=0;
}