//Attacker/forward for RoboCup Junior soccer, revised version
//David Wyatt 12/5/2003
//Uses separate calibration programme calibrator.nqc which MUST be run first

//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 CHECK_TIME 40
#define TIMEOUT 20
#define REV_TIME 7
#define WHOLE_TURN 15
#define PART_TURN 3
#define SMALL_PAUSE 10
#define BIG_PAUSE 30
#define GRADIENT_TIME 2

//Timer numbers used in different bits of the programme
#define SPIN_TIMER 0
#define PUSH_TIMER 1
#define FORWARD_TIMER 2


int home, away, field_brightness; //Global calibration variables set by calibrator.nqc
int position = 50; //Percentage of the way from own end to opponent's end, calculated from light sensor readings
int old_position = 50; //Previous position
int direction = 0; //Whether turn to left or right to get past an obstacle
int previous = 0; //Previous light level

task main()
{
 init();
 field_brightness -= 5;
 until (BUMPER == 0);  //Wait until told to continue
 until (BUMPER == 1);
 until (BUMPER == 0);

 while (true) //Main football-playing loop
 {
  if (BUMPER == 1) //If jammed against wall with ball already
  {
   reverse();
  }
  do //Keep checking direction of brightest thing until you bump into something
  {
   find_ball();
   locate();
   old_position = position;
   OnFwd(LEFT + RIGHT); //Advance towards ball
/*   Wait(GRADIENT_TIME * 10);
  locate();
  if (position < old_position) //Facing towards own end?
  {
    PlaySound(SOUND_DOWN);
    past_ball();
    continue;
  }*/
   until (BUMPER == 1 || LIGHT_FWD > field_brightness); //Look again if lost ball
  }
  while (BUMPER == 0);
  if (LIGHT_FWD < field_brightness) //Is it a ball?
  {
   /*locate();
   if (position > old_position) //Facing towards opponent's end?*/
   {
    PlaySound(SOUND_UP);
    push_ball();
   }
  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 find_ball()
{
 while(1)
 {
  OnFwd(LEFT);
  OnRev(RIGHT);
  ClearTimer(SPIN_TIMER);
  until(Timer(SPIN_TIMER) > WHOLE_TURN || LIGHT_FWD < field_brightness);
  if(LIGHT_FWD < field_brightness)
  {
   return;
  }
  forward(BIG_PAUSE);
 }
 Off(LEFT + RIGHT);
}

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

void past_ball()
{
 reverse();
 turn(direction);
 forward(SMALL_PAUSE);
 turn((direction + 1) % 2);
 OnFwd (LEFT + RIGHT);
 ClearTimer(FORWARD_TIMER);
 until(Timer(FORWARD_TIMER) >= BIG_PAUSE)
 {
  if (BUMPER == 1)
  {
   reverse();
   turn(direction);
   direction = (direction + 1) % 2;
   return;
  }
 }
 Off(LEFT + RIGHT);
}

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

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

void forward(int time)
{
 ClearTimer(FORWARD_TIMER);
 OnFwd(LEFT + RIGHT);
 until(Timer(FORWARD_TIMER) >= time)
 {
  if (BUMPER == 1)
  {
   reverse();
   turn(direction);
   break;
  }
 }
 Off(LEFT + RIGHT);
}

void push_ball()
{
 OnFwd(LEFT + RIGHT);
 ClearTimer(PUSH_TIMER);
 until(Timer(PUSH_TIMER) >= BIG_PAUSE)
 {
  previous = LIGHT_FLOOR;
  Wait(GRADIENT_TIME * 10);
  if (abs(LIGHT_FLOOR - previous) < ERROR) //Is the ball is against a wall?
  {
   OnRev(LEFT + RIGHT);
   Wait(10);
   turn(0);
   turn(0);
   turn(0);
   break;
  }
 }
 Off(LEFT + RIGHT);
}