#pragma noinit
#define OldFwd(a,b)   do {OnFwd(a); SetPower(a,b);} while (false)
#define OldRev(a,b)   do {OnRev(a); SetPower(a,b);} while (false)

/*
 * Lightseeker
 * ===========
 *
 * Written by Mark Overmars
 *
 * The light seeker has two motors to drive the rear wheels.
 * On the top there is a micromotor, connected to OUT_B that
 * rotates a lightsensor left and right. A position sensor
 * messures the direction of the light sensors. Finally, there
 * are two touch sensors at the front.
 *
 * The behavior of the robot is as follows. It constantly
 * determines the direction of the most light using the 
 * rotating light sensor. Based on the direction of the light
 * it steers the robot towards the light. If the touch sensors
 * touch something for the first time, the robot backs up a bit.
 * If they touch something again, it is assumed the goal is
 * reached.
 *
 * Mapping:
 *   OUT_A: left wheel
 *   OUT_B: micromotor
 *   OUT_C: right wheel
 *    IN_1: position sensor
 *    IN_2: light sensor
 *    IN_3: both touch sensors
 *
 */

#define ROTATE_SPEED 8
#define TURN_TIME 30
#define DRIVE_SPEED 1

#define Forward     OldRev(OUT_A,DRIVE_SPEED); OldFwd(OUT_C,DRIVE_SPEED);
#define Backward    OldFwd(OUT_A,DRIVE_SPEED); OldRev(OUT_C,DRIVE_SPEED);
#define RotateLeft  OldFwd(OUT_A,DRIVE_SPEED); OldFwd(OUT_C,DRIVE_SPEED);
#define RotateRight OldRev(OUT_A,DRIVE_SPEED); OldRev(OUT_C,DRIVE_SPEED);

int maxlight;  // most light found
int maxdir;    // direction for most light
int touchnumb; // number of touches
int backup;    // while backing up no light correction
int ttt;       // temporary variable

task check_touch()
// Checks whether touch sensors are touched and takes action
{
  while (true)
  {
    if (SENSOR_3 == 1)
    {
      if (touchnumb > 2)
      {
        // Stop everything else
        Off(OUT_A+OUT_C);
        stop rotate;
        stop check_light;
        // Turn light sensor forward
        if (SENSOR_1 <0) OldFwd(OUT_B,8);
        if (SENSOR_1 >0) OldRev(OUT_B,8);
        until(SENSOR_1 == 0);
        Off(OUT_B);
        stop check_touch;
      }
      else
      {
        // backup a bit and try from there
        backup = 1;
        Backward;
        Wait(200);
        Off(OUT_A+OUT_C);
        backup = 0;
        touchnumb += 1;
      }
    }
  }
}

task check_light()
// Constantly keeps track of the best light direction
{
  while(true)
  {
    if (SENSOR_2 < maxlight)
    {
      maxlight = SENSOR_2;
      maxdir = SENSOR_1;
    }
  }
}

sub changedir()
// Corrects the direction of the robot based on the light direction.
{
   if (maxdir > 0)
    {
      ttt = maxdir;
      ttt *= TURN_TIME;
      RotateLeft;
      Wait(ttt);
      PlaySound(1);
    }  
    if (maxdir < 0)
    {
      ttt = 0;
      ttt -= maxdir;
      ttt *= TURN_TIME;
      RotateRight;
      Wait(ttt);
      PlaySound(2);
    }  
    Forward;
}

task rotate()
// Rotates the light sensor left and right
{
  OldFwd(OUT_B,ROTATE_SPEED);
  until(SENSOR_1 >= 8);
  OldRev(OUT_B,ROTATE_SPEED);
  while(true)
  {
    maxlight = 1000;
    OldRev(OUT_B,ROTATE_SPEED);
    until(SENSOR_1 <= -8);
    Off(OUT_B);
    if (backup == 0) changedir();
    maxlight = 1000;
    OldFwd(OUT_B,ROTATE_SPEED);
    until(SENSOR_1 >= 8);
    Off(OUT_B);
    if (backup == 0) changedir();
  }
}


task main()
// Just initialises things
{
  // Set up the sensors
  SetSensor(SENSOR_1,SENSOR_ROTATION);
  ClearSensor(SENSOR_1);
  SetSensor(SENSOR_2, _SENSOR_CFG(SENSOR_TYPE_LIGHT,  SENSOR_MODE_RAW));
  SetSensor(SENSOR_3, SENSOR_TOUCH);
  // Start it all
  touchnumb = 0;
  backup = 0;
  start rotate;
  start check_light;
  start check_touch;
}