//Attacker/forward for RoboCup Junior soccer
//David Wyatt 28/02/2003

//N.B. Light sensors set to raw mode, where light == low value

#define LIGHT_FLOOR SENSOR_1 //Uncollimated light sensor pointing downwards to detect position on field from grayscale gradient
#define LIGHT_FWD SENSOR_2 //Collimated light sensor pointing forwards to detect ball
#define BUMPER SENSOR_3 //Front bumper to detect ball and obstacles
#define LEFT OUT_A //Left motor - FWD == forwards (i.e. not reversed)
#define RIGHT OUT_C //Right motor - FWD == forwards (i.e. not reversed)
#define ERROR 2 //Noise value in raw light sensor reading

//All these in deciseconds
#define CHECKTIME 40
#define STOPTIME 0
#define TIMEOUT 20
#define REVTIME 7
#define WHOLETURN 15
#define PARTTURN 4
#define SMALLPAUSE 10
#define BIGPAUSE 30

#define MAX_BUMPS 1
#define BALL_BRIGHTNESS 780 //Needs calibration, possibly

int home = 0;
int away = 0;
int position = 50; //Percentage of the way from own end to opponent's end, calculated from light sensor readings
int positionold; //Previous position
int direction = 0; //Whether turn to left or right to get past an obstacle

task main()
{
 int previous;
 init();
 calibrate();
 until (BUMPER == 0);  //Wait until told to continue
 until (BUMPER == 1);
 until (BUMPER == 0);

 while (true) //Main football-playing loop
 {
  do //Keep checking direction of brightest thing until you bump into something
  {
   if (BUMPER == 1) //If jammed against wall with ball already
   {
    reverse();
   }
   findball();
   quicklocate();
   positionold = position;
   ClearTimer(1);
   OnFwd(LEFT + RIGHT); //Advance towards ball
   until (BUMPER == 1 || Timer(1) >= CHECKTIME);  //Look again if too far away
  }
  while (BUMPER == 0);
  if (LIGHT_FWD < BALL_BRIGHTNESS) //Is it a ball?
  {
   quicklocate();
   if (position > positionold) //Facing towards opponent's end?
   {
    PlaySound(SOUND_UP);
    OnFwd(LEFT + RIGHT);
    ClearTimer(1);
    until(Timer(1) >= BIGPAUSE)
    {
     previous = LIGHT_FWD;
     Wait(20);
     if (abs(LIGHT_FWD - previous) < ERROR) //Is the ball is against a wall?
     {
      OnRev(LEFT + RIGHT);
      Wait(10);
      turn(0);
      turn(0);
      turn(0);
      until(Timer(1) >= BIGPAUSE);
     }
    }
    Off(LEFT + RIGHT);
    /*Wait(200);*/
   }
   else //Facing towards own end
   {
    PlaySound(SOUND_DOWN);
    pastball();
   }
  }
  else //Not a ball == a wall or a ball at edge of bumper
  {
   reverse(); //Reverse away from whatever you've bumped into
  }
 }
}

void init()
{
 SetSensorType(LIGHT_FLOOR, SENSOR_TYPE_LIGHT);
 SetSensorMode(LIGHT_FLOOR, SENSOR_MODE_RAW);
 SetSensorType(LIGHT_FWD, SENSOR_TYPE_LIGHT);
 SetSensorMode(LIGHT_FWD, SENSOR_MODE_RAW);
 SetSensor(BUMPER, SENSOR_TOUCH);
}

void calibrate()
{
 until (abs(home - away) > ERROR)
 {
  until (BUMPER == 0);
  until (BUMPER == 1);
  home = LIGHT_FLOOR;
  PlaySound(SOUND_CLICK);
  until (BUMPER == 0);
  until (BUMPER == 1);
  away = LIGHT_FLOOR;
  if (abs(home - away) <= ERROR) {PlaySound(SOUND_DOWN);}
 }
 PlaySound(SOUND_UP);
}

void findball()
{
 int highest = 0, highest2;
 OnFwd(LEFT);
 OnRev(RIGHT);
 do
 {
  highest2 = 1023;
  ClearTimer(0);
  until(Timer(0) > WHOLETURN || LIGHT_FWD <= highest + ERROR)
  {
   if (LIGHT_FWD <= highest2 - ERROR) {highest2 = LIGHT_FWD;}
  }
  highest = highest2;
 }
 while (Timer(0) >= WHOLETURN);
 Off(LEFT + RIGHT);
 Wait(STOPTIME * 10);
}

void locate()
{
 int highest;
 do
 {
  highest = 1023;
  OnFwd(LEFT);
  OnRev(RIGHT);
  ClearTimer(0);
  while(Timer(0) <= WHOLETURN)
  {
   if (LIGHT_FLOOR <= highest - ERROR) {highest = LIGHT_FLOOR;}
  }
  Toggle(LEFT + RIGHT);
  ClearTimer(0);
  until (LIGHT_FLOOR <= highest + ERROR || Timer(0) >= TIMEOUT);
 }
 while (Timer(0) >= TIMEOUT);
 Off(LEFT + RIGHT);
 Wait(STOPTIME * 10);
 position = abs(highest - home) * 100 / abs(away - home);
}

void quicklocate()
{
 position = (LIGHT_FLOOR - home) * 100 / (away - home);
}

void beepPosition()
{
 int tens = position / 10, units = position - (tens * 10), i;
 for (i=0; i < tens; i++)
 {
  PlaySound(SOUND_CLICK);
  Wait(20);
 }
 /*Wait(30);
 for (i=0; i < units; i++)
 {
  PlaySound(SOUND_CLICK);
  Wait(20);
 }*/
}

void wander()
{
 int i;
 OnRev(LEFT);
 OnFwd(RIGHT);
 Wait(Random(100));
 ClearTimer(0);
 Fwd(LEFT + RIGHT);
 for(i=0; i < MAX_BUMPS && Timer(0) < 30; i++)
 {
  until (BUMPER == 0);
  until (BUMPER == 1);
 }
 reverse();
}

void pastball()
{
 reverse();
 turn(direction);
 OnFwd(LEFT + RIGHT);
 ClearTimer(1);
 until(Timer(1) >= SMALLPAUSE)
 {
  if (BUMPER == 1)
  {
   reverse();
   turn(direction);
   return;
  }
 }
 turn((direction + 1) % 2);
 OnFwd (LEFT + RIGHT);
 ClearTimer(1);
 until(Timer(1) >= BIGPAUSE)
 {
  if (BUMPER == 1)
  {
   reverse();
   turn(direction);
   direction = (direction + 1) % 2;
   return;
  }
 }
 Off(LEFT + RIGHT);
}

void reverse()
{
 OnRev(LEFT + RIGHT);
 Wait(REVTIME * 10);
 Off(LEFT + RIGHT);
}

void turn(int dir)
{
 On(LEFT + RIGHT);
 if (dir == 0)
 {
  Fwd (LEFT);
  Rev (RIGHT);
 }
 else
 {
  Rev (LEFT);
  Fwd(RIGHT);
 }
 Wait(PARTTURN * 10);
 Off(LEFT + RIGHT);
}