//*!!Sensor,    S4,                sonar, sensorSONAR9V,      ,                  !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
// LookBot
//
// The LookBot uses two motors (B, C) for locomotion and steering and another
// to control its radar (ultrasonar) to look for and avoid obstacles.

#pragma platform("NXT")

const tSensors gc_touch = (tSensors)S1;
const tSensors gc_sonar = (tSensors)S4;

//
// Special "distance" values returned by the SONAR device driver to indicate error conditions
//
typedef enum
{
  sonarNoSensorConnected = 254,
  sonarOutOfRange        = 255
} TSonarErrorCodes;


// Global constants (gc_***)
//
const int gc_headPower     = 35;  // Radar motor power
const int gc_headAngle     = 75;  // Radar goes -headAngle to headAngle
const int gc_headBacklash  = 10;  // Adjust for interal gear backlash to get head close to forward

const int gc_drivePower    = 45;  // Drive wheel power
const int gc_tooClose      = 30;  // Distance at which to consider evasive action
const int gc_muchTooClose  = 20;  // Distance at which to consider evasive action

const int gc_motorBalanceThreshold = 5;	// Allow this much (degrees) drift before compensating

// Globals (g_***) for communication between tasks (I am not comfortable with
// doing this, but it seems to be the way to do it)

bool g_alive    = true;
int  g_hitCount = 0;

// Radar
//
// The radar task continually scans left and right looking for nearby obstacles
// Resets dead man's handle
//
task Radar()
{
  while (true)
  {
    // Look right
    motor[motorA] = gc_headPower;

    do
    {
    } while (nMotorEncoder[motorA] < gc_headAngle);

    motor[motorA] = 0;
    wait1Msec(200);

    // Look left
    motor[motorA] = -gc_headPower;

    do
    {
    } while (nMotorEncoder[motorA] > -gc_headAngle);

    motor[motorA] = 0;
    wait1Msec(200);

    // Return to home position
    motor[motorA] = gc_headPower;

    do
    {
    } while (nMotorEncoder[motorA] < -gc_headBacklash);

    g_alive = true;
  }
}

// TurnLeft
//
// Execute a turn left
//
inline void TurnLeft(int time, int rnd)
{
  motor[motorB] = -gc_drivePower;
  motor[motorC] =  gc_drivePower;

  wait1Msec(time + random(rnd));
}

// TurnRight
//
// Execute a turn right
//
inline void TurnRight(int time, int rnd)
{
  motor[motorB] =  gc_drivePower;
  motor[motorC] = -gc_drivePower;

  wait1Msec(time + random(rnd));
}

// Reverse
//
// Back up for a short while
//
inline void Reverse(int time, int rnd)
{
  motor[motorB] = -gc_drivePower;
  motor[motorC] = -gc_drivePower;

  wait1Msec(time + random(rnd));
}

// Squirm
//
// Try to wriggle out of a dark corner
//
inline void Squirm()
{
  // Try to get out of a spot
  Reverse(500, 500);

  if (random(1) == 1)
    TurnRight(500, 500);
  else
    TurnLeft(500, 500);
}

// FullAhead
//
// Drive forward in a straight line.  Compensate for drift between motors
//
task FullAhead()
{
  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;

  motor[motorB] = gc_drivePower;
  motor[motorC] = gc_drivePower;

  while (true)
  {
    int motorEncoderB = nMotorEncoder[motorB];
    int motorEncoderC = nMotorEncoder[motorC];

    if (abs(motorEncoderB - motorEncoderC) > gc_motorBalanceThreshold)
    {
      if (motorEncoderB < motorEncoderC)
      {
        motor[motorC] = 0.9 * gc_drivePower;
        motor[motorB] = gc_drivePower;
      }
      else
      {
        motor[motorB] = 0.9 * gc_drivePower;
        motor[motorC] = gc_drivePower;
      }
    }

    wait1Msec(100);

    g_hitCount = 0;
  }
}

void FindOpening()
{
  int maxDist  = 0;
  int maxAngle = 0;
  int i;

  for (i = 0; i < 25; ++i)
  {
    int angle = nMotorEncoder[motorA];
    int dist  = SensorRaw[gc_sonar];

    if (dist == sonarNoSensorConnected)
      StopAllTasks();

    if (dist == sonarOutOfRange)
      dist = 253;

    if (dist > maxDist)
    {
      maxDist  = dist;
      maxAngle = angle;
    }

    wait1Msec(90);
  }

  nxtDisplayTextLine(1, "MaxDist  = %d", maxDist);
  nxtDisplayTextLine(2, "MaxAngle = %d", maxAngle);

  nMotorEncoder[motorB] = 0;
  nMotorEncoder[motorC] = 0;

  if (maxDist < gc_tooClose)
  {
    Squirm();
  }
  else
  {
    if (maxAngle < 0)
    {
    	maxAngle = -maxAngle;

      // Rotate right
      motor[motorB] =  gc_drivePower;
      motor[motorC] = -gc_drivePower;

      while (nMotorEncoder[motorB] < maxAngle)
        wait1Msec(10);
    }
    else
    {
      // Rotate left
      motor[motorB] = -gc_drivePower;
      motor[motorC] =  gc_drivePower;

      while (nMotorEncoder[motorC] < maxAngle)
        wait1Msec(10);
    }
  }
}

// StopAhead
//
// Terminate the FullAhead task and stop motors.
//
inline void StopAhead()
{
  StopTask(FullAhead);

  motor[motorB] = 0;
  motor[motorC] = 0;
}

// Proximity
//
//
task Proximity()
{
  while (true)
  {
    int dist  = SensorRaw[gc_sonar];
    int angle = nMotorEncoder[motorA];

    if ((dist < gc_tooClose && abs(angle) < 20) || dist < gc_muchTooClose)
    {
      StopAhead();
    	g_hitCount += 1;

    	FindOpening();

      StartTask(FullAhead);
    }
  }
}

// FrontHit
//
//
task FrontHit()
{
  while (true)
  {
  	if (SensorValue[gc_touch] != 0)
  	{
	    StopTask(Proximity);
      StopAhead();

  		Squirm();

      StartTask(FullAhead);
      StartTask(Proximity);
  	}
  }
}

// Init
//
//
void Init()
{
  // Motors should not float
  bFloatDuringInactiveMotorPWM = false;

  // Configure the ultrasound sensor (9V might be better?)
  SetSensorType(gc_sonar, sensorSONAR9V);
  SetSensorType(gc_touch, sensorTouch);

  // Reset motor encoders
  nMotorEncoder[motorA] = 0;

  StartTask(Radar);

  StartTask(FullAhead);
  StartTask(Proximity);
  StartTask(FrontHit);
}

// main
//
//
task main()
{
  Init();

  while (true)
  {
    g_alive = false;

    // Twiddle thumbs while other tasks get on with navigating
    wait1Msec(2000);

    // If not alive, then the Radar has stalled
    // If g_hitCount too hight then we are making repeated futile rotations
    if (!g_alive || g_hitCount > 3)
    {
      StopTask(FrontHit);
	    StopTask(Proximity);
      StopAhead();

      Squirm();

      StartTask(FullAhead);
      StartTask(Proximity);
      StartTask(FrontHit);
    }

    // Stop bot from powering down
    alive();
  }
}