#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 player robot code for Robot B(slave)
// Expects team leader code to be installed and run on Robot A (master)

task main()
{
  int dir;
  int acS1, acS2, acS3, acS4, acS5;
  string messageReceived, messageToSend;
  int 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,"B 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);
  distanceB = max2(temp1, temp2);
  nxtDisplayTextLine(5,"max = %d", distanceB);

  while(true)
  {
    // wait for requests (messages) from team leader Robot A
    // only act on requests that are directed to Robot B
    ReceiveString(messageReceived);
    nxtDisplayTextLine(7, messageReceived);

    if (StringFind(messageReceived, "B send distance") >= 0)
    {
      StringFormat(messageToSend, "%d", distanceB);
      SendString(messageToSend);
    }

    if (StringFind(messageReceived, "B move forward") >= 0)
    {
      motor[motorB] = 50;
      motor[motorC] = 50;
      wait1Msec(1000);
      motor[motorB] = 0;
      motor[motorC] = 0;

    }
  }
}