#pragma config(Sensor, S3,     Light,          sensorLightActive)
#pragma config(Motor,  motorA,          Gripper,       tmotorNormal, PIDControl, encoder)
#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 XBees
#include "XbeeTools.h"

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


   //Receive encoder count from NXT A
   ReceiveString(data);

   //Calculate total encoder distance
   count=atoi(data);
   count=7-count;
   encoder=(((count*9)/7.0685775)*360);

   //Set motor encoders equal to 0;
   nMotorEncoder[LeftMotor]=0;
   nMotorEncoder[RightMotor]=0;

   //Set encoder targets equal to calculated distance
   nMotorEncoderTarget[LeftMotor]=encoder;
   nMotorEncoderTarget[RightMotor]=encoder;

   //Turn motors on
   motor[LeftMotor]=50;
   motor[RightMotor]=50;

   //Run motors until distance is reached
   while((nMotorRunState[LeftMotor] != runStateIdle) && (nMotorRunState[RightMotor] != runStateIdle))
     {/*Idle loop*/}

   //Turn motors off if not off already
   motor[LeftMotor]=0;
   motor[RightMotor]=0;

   //Reset gripper encoder and open it up
   nMotorEncoder[Gripper]=0;
   while(nMotorEncoder[Gripper]<=55)
      {
       motor[Gripper]=10;
      }
   motor[Gripper]=0;

   //Set motor encoders equal to 0;
   nMotorEncoder[LeftMotor]=0;
   nMotorEncoder[RightMotor]=0;

   //Set encoder targets equal to needed distance
   nMotorEncoderTarget[LeftMotor]=370;
   nMotorEncoderTarget[RightMotor]=370;

   //Turn motors on
   motor[LeftMotor]=50;
   motor[RightMotor]=50;

   //Run motors until distance is reached
   while((nMotorRunState[LeftMotor] != runStateIdle) && (nMotorRunState[RightMotor] != runStateIdle))
     {/*Idle loop*/}

   //Turn motors off if not off already
   motor[LeftMotor]=0;
   motor[RightMotor]=0;

   //Close gripper
   while(nMotorEncoder[Gripper]>=0)
      {
       motor[Gripper]=-10;
      }
   motor[Gripper]=0;
   wait1Msec(1000);


   //If the ball is red;
   if (SensorValue(Light)>=35 && SensorValue(Light)<=53)
        {
          //Send color to NXT A
          data="Red";
          SendString(data);
          nMotorEncoder[LeftMotor]=0;
          nMotorEncoder[RightMotor]=0;

          //Set encoders to pivot in place to the left and point towards goal
          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 ball is blue;
   if (SensorValue(Light)>=24 && SensorValue(Light)<=34)
        {
          //Send color to NXT A
          data="Blue";
          SendString(data);
          nMotorEncoder[LeftMotor]=0;
          nMotorEncoder[RightMotor]=0;

          //Set encoders to pivot in place to the right and point towards goal
          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;

        }

       nMotorEncoder[LeftMotor]=0;

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

        //Stop the motors
        motor[LeftMotor]=0;
        motor[RightMotor]=0;
}