#pragma config(Sensor, S1,     IRSeeker,              sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "HTIRS2-driver.h"
#include "XbeeTools.h"

// Initial setup: 2 robots with IR Seeker sensors are pointing towards
// an IR ball (set at 600Hz).  Each robot measures the relative distance to
// the IR ball using the intensity. (This will generally be sensor 5 if robot is
// facing ball). The team leader (Robot A) asks other robot (team member B)
// for its distance from the ball, and team leader robot determines which robot
// is closest to the ball.  The closest robot moves forward to the ball
// and stops (or "kicks" the ball).

// Team leader Robot A (master) code
// Download B version on Robot B (slave)
// To operate, start Robot B before Robot A

task main()
{
  int dir;
  int acS1, acS2, acS3, acS4, acS5;
  string messageReceived;
  int distanceA, distanceB;
  tHTIRS2DSPMode _mode = DSP_600; //set to 600 or 1200Hz; must match IR ball
  HTIRS2setDSPMode(IRSeeker, _mode);
  InitRS485();
  wait1Msec(100);

  // get IR seeker sensor data (direction and 5 intensities)
  dir = HTIRS2readACDir(IRSeeker);
  HTIRS2readAllACStrength(IRSeeker, acS1, acS2, acS3, acS4, acS5);

  nxtDisplayTextLine(1,"A dir = %d", dir);
  nxtDisplayTextLine(3,"%d %d %d %d %d", acS1, acS2, acS3, acS4, acS5);

  // find max intesity level  (high intensity means IR ball is close)
  int temp1 = max3(acS1, acS2, acS3);
  int temp2 = max2(acS4, acS5);
  distanceA = max2(temp1, temp2);

  nxtDisplayTextLine(5,"max = %d", distanceA);

  // ask other robot (Robot B) to send back the distances to the ball

  nxtDisplayTextLine(7,"Send req to B");
  wait1Msec(1500);
  SendString("B send distance");  // ask Robot B for its distance to ball
  ReceiveString(messageReceived);
  // distanceB is max intensity level reading from Robot B
  distanceB = atoi(messageReceived);  // distance is related to intensity reading

  wait1Msec(1500);

  // find which Robot is closest to the ball (greatest intensity reading)
  if ((distanceA >= distanceB))	  {
    // Robot A is closest; Robot A moves forward
    nxtDisplayTextLine(7,"Robot A closest");
    wait1Msec(1500);
    motor[motorB] = 50;
    motor[motorC] = 50;
    wait1Msec(1000);
    motor[motorB] = 0;
    motor[motorC] = 0;
  }

  else
  {
    // Robot B is closest; send message to Robot B to move forward
    nxtDisplayTextLine(7,"Robot B closest");
    wait1Msec(1500);
    SendString("B move forward");
  }

}