/* notes:
   spin(POSITIVE NUMBER) for clockwise rotation.
*/

int main() {

  int side, clicks, plan;

  /* enable_servos();
  init_drive(); 
  init_hinge(); 
  close_hinge();  */
 
  /* start_press(); */

  start_machine(16);

  /* enable_servos();
  init_drive(); 
  init_hinge(); 
  close_hinge(); */

  side = orient();
  sleep(.5);
  
  open_hinge();
  
  sleep(.5);

  /*printf("Moving Back.\n");*/

  drive_backwall();
  drive_alignforward();

  /*printf("Aligned.\n");*/
  
  sleep(.5);
  clicks = drive_sideways_first_ball(side);
  drive_back_sideways(clicks, side);

  sleep(.5);
  steer_straight();
  sleep(.5);

  plan = drive_find_barrier(side);
  if(plan == PLAN_A)
    {
      printf("PLAN A\n");
      sleep(1.0);
      plan_a(side);
    }
  else if(plan == PLAN_B)
    {
      printf("PLAN B\n");
      sleep(1.0);
      plan_b(side);
    }
  else if(plan == PLAN_C)
    {
      printf("PLAN C\n");
      sleep(1.0);
      plan_c(side);
    }
  else
    {
      printf("1 d0n't have 1337 p1anz!!11\n");
      /* Assume Plan C */
      sleep(1.0);
      plan_c(side);
    }

  steer_halfway();
  sleep(0.5);
  steer_sideways();
  sleep(2.0);

  /* Get in the way of the opponent */

  if(side == SIDE_WHITE)
    drive_sideways(-100);
  else
    drive_sideways(100);

  sleep(5.0);

  ao();

}

/* The plans assume the robot is straight back against the back. */

void plan_a(int side) {
  /* Plan A.  Drive around the outside of the barrier in a large arc. */

  steer_sideways();
  sleep(.5);

  if(side == SIDE_WHITE)
    drive_sideways(-40);
  else
    drive_sideways(40);

  sleep(3.0);
  if(side == SIDE_WHITE)
    drive_sideways(20);
  else
    drive_sideways(-20);
  sleep(.4);

  drive_sideways(0);

  steer_straight();

  /* Drive forward for a while until we think we're past the barrier. */

  drive_straight_clicks(80, 105, 500);

  /* Now arc around about 90 degrees */

  if(side == SIDE_WHITE)
    drive_around_left(80, 35, 300);
  else
    drive_around_right(80, 35, 300);

  /* Now drive to the trough */

  drive_straight_clicks(100, 120, 300);
}


void plan_b(int side) {
  /* Plan B.  Drive up near the trough and stick the balls in. */

  /* Near the trough is left on white, right on black. */

  steer_sideways();
  sleep(.5);

  if(side == SIDE_WHITE)
    drive_sideways(30);
  else
    drive_sideways(-30);

  sleep(2.0);

  if(side == SIDE_WHITE)
    drive_sideways(-30);
  else
    drive_sideways(30);
  sleep(0.3);

  drive_sideways(0);

  steer_straight();
  
  /* Drive forward several clicks and then turn */

  drive_straight_clicks(60, 80, 500);

  /* Now turn and stick balls in */

  if(side == SIDE_WHITE)
    drive_around_left(80, 1000, 300);
  else
    drive_around_right(80, 1000, 300);
}


void plan_c(int side) {
  int timeout = 0;

  /* Plan C.  Catch the near balls and ignore the far ones. */

  /* Move to the outside like plan A */
  steer_sideways();
  sleep(.5);

  if(side == SIDE_WHITE)
    drive_sideways(-40);
  else
    drive_sideways(40);

  sleep(3.0);
  drive_sideways(0);

  steer_straight();
  sleep(0.5);


  /* Drive forward just a little bit before turning */

  drive_straight_clicks(80, 5, 100);

  /* Now turn */

  if(side == SIDE_WHITE)
    drive_around_left(80, 40, 300);
  else
    drive_around_right(80, 40, 300);

  drive_forward(100);

  /*
  sleep(1.0);

  drive_forward(60);
  */

  while(!(digital(BUMP_LEFT_WHEEL) && digital(BUMP_RIGHT_WHEEL)) && timeout++ < 500)
    {
      sleep(.01);
    }

  drive_forward(0);
}


void doh() {
  printf("D'oh!\n");
  ao();
  disable_servos();
}