/* final.c */

void main()
{
  int config;
  
  enable_servos();
  close_claw();

  printf("final.c         ");
  printf("Press Start\n");
      
  start_press();

  calibrate(0);
  printf("\nReady...\n");
  start_press();
  
  start_machine(START_LIGHT);

  enable_encoder(ENCODER); /* enable the shaft encoder */

  enable_servos();
  close_claw();

  color = whoami();
  face_block(); 
  
  measured_forward(5); 
  block_test();

  wall_align();
  measured_forward(3);
  sleep(.50);

  /* orient(); */

  if (color == 3)
    left_90();
  else
    right_90();

  open_claw_straight();
  sleep(.25);

  if (color == 3)
    black();
  else
    white();
}


/* This function tests if the block is in the two nearest locations */
int block_test()
{
  sleep(1.0);
  if(digital(FRONT_LEFT_BUMPER_1) || digital(FRONT_LEFT_BUMPER_2))
    {
      config = 2;
      return 2;
    }

  if(digital(FRONT_RIGHT_BUMPER_1) || digital(FRONT_RIGHT_BUMPER_2))
    {
      config = 1;
      return 1;
    }

  config = -1;
  return -1;
}


/* the strategy code for the black robot */    
void black()
{    
  int afr, afl;
  int count = 0;
  printf("Step 1...\n");
  
  /* Go forward until we out of the start area */
  analog(FRONT_LEFT);
  while(analog(FRONT_LEFT) > FL_threshold)
    {
      forward();
    }

  printf("Step 2...\n");
  /* go until we reach the black line and then close the claw
     if the ball hits the internal sensors we also stop */
  analog(FRONT_LEFT);
  afl = analog(FRONT_LEFT);
  analog(FRONT_RIGHT);
  afr = analog(FRONT_RIGHT);

  while((analog(FRONT_LEFT) < FL_threshold ) 
	&& (analog(FRONT_RIGHT) < FR_threshold))
    {
      
      forward();

      if(digital(RIGHT_BALL_SENSOR) || digital(LEFT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}

      if (count == 5000)
	break;

      count++;
    }

  close_claw();
  ao();
  
  /* we stop then reverse to hit the rear ball */
  sleep(.25);
  printf("Step 3...\n");
  measured_reverse(82);

  /* go forward a little bit to give us room to turn */
  measured_forward(10);
  left_180();
  sleep(1.0);
  /* open claw and then back up slightly */
  open_claw_straight();
  measured_reverse(20);
  sleep(.50);

  /* close claw then go forward to hit ball over lip */
  close_claw();
  measured_forward(35);

  /* back us up a bit before we go on */
  measured_reverse(10);

  if (config == 1)
    black_1();
  
  if (config == 2)
    black_2();
  
  if (config == -1)
    black_3_4();

  
}

/* Common actions for configuration 1 and 2 */
void black_1()
{
  int count = 0;
  wall_align();
  measured_forward(3);
  left_90();
  wall_align();

  measured_forward(60);
  right_90();
  wall_align();

  open_claw_straight();
  sleep(.25);

  measured_forward(55);
  measured_reverse(10);
  left_180();
  wall_align();

  measured_forward(47);
  
  left_90();
  open_claw_straight();
  sleep(.25);

  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();
  
      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	}

      if (count == 10000)
	break;

      count++;
    }

  ao();
  right_180();
  wall_align();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  right_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();

}

void black_2()
{
  int count;

  left_90();
  printf("\nWaiting...\n");

  wall_align();

  measured_forward(60);
  left_90();
  wall_align();
  open_claw_straight();
  sleep(.25);

  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();

      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}

      if (count == 10000)
	break;

      count++;
    }
  
  ao();
  sleep(.25);
  right_180();
  sleep(.75);
  open_claw_straight();
  measured_forward(35);

  measured_reverse(15);
  close_claw();
  left_180();
  wall_align();

  count = 0;
  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();

      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}

      if (count == 10000)
	break;

      count++;
    }

  ao();
  close_claw();
  right_90();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  right_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();
}


/* function for handling  configurations 3 and four */
void black_3_4()
{
  wall_align();
  measured_forward(3);
  left_90();
  wall_align();
  
  measured_forward(30);
  right_90();
  sleep(.25);
  wall_align();
  sleep(.25);
  open_claw_straight();
  sleep(.25);

  measured_forward(55);
  left_180();
  wall_align();
 
  measured_forward(24);
  open_right();

  if (digital(FRONT_RIGHT_BUMPER_1) || digital(FRONT_RIGHT_BUMPER_2))
    config = 3;
  else
    config = 4;
  
  close_right();
  
  if(config == 3)
    measured_forward(23);

  right_90();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  right_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();
}


/* strategy code for white player */
void white()
{
  int afl, afr;
  int count = 0;
  printf("Step 1...\n");
  
  /* Go forward until we out of the start area */
  analog(FRONT_LEFT);
  while(analog(FRONT_LEFT) > FL_threshold)
    {
      forward();
    }

  printf("Step 2...\n");
  /* go until we reach the black line and then close the claw
     if the ball hits the internal sensors we stop also */

  analog(FRONT_LEFT);
  afl = analog(FRONT_LEFT);
  analog(FRONT_RIGHT);
  afr = analog(FRONT_RIGHT);

  while((afl < FL_threshold ) 
	&& (afr < FR_threshold))
    {
      
      forward();

      if(digital(RIGHT_BALL_SENSOR) || digital(LEFT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}

      if (count == 5000)
	break;

      count++;
    }

  close_claw();
  ao();
  
  /* we stop then reverse to hit the rear ball */
  sleep(.25);
  printf("Step 3...\n");
  measured_reverse(82);

  /* go forward a little bit to give us room to turn */
  measured_forward(10);
  right_180();
  sleep(1.0);
  /* open claw and then back up slightly */
  open_claw_straight();
  measured_reverse(20);
  sleep(.50);

  /* close claw then go forward to hit ball over lip */
  close_claw();
  measured_forward(35);

  /* back us up a bit before we go on */
  measured_reverse(10);

  if (config == 1)
    white_1();

  if(config == 2)
    white_2();

  if (config == -1)
    white_3_4(); 
}

void white_1()
{
  int count = 0;

  wall_align();
  measured_forward(3);
  right_90();
  wall_align();

  measured_forward(60);
  left_90();
  wall_align();

  open_claw_straight();
  sleep(.25);

  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();     
    }

  ao();

  measured_reverse(10);
  right_180();
  wall_align();

  measured_forward(47);
  
  right_90();
  open_claw_straight();
  sleep(.25);

  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();

      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	}

      if(count == 10000)
	break;

      count++;

    }

  ao();
  left_180();
  wall_align();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  left_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();

}

void white_2()
{
  int count = 0;
  right_90();
  wall_align();

  measured_forward(60);
  right_90();
  wall_align();
  open_claw_straight();
  sleep(.25);

  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();

      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}

      if(count == 10000)
	break;

      count++;
    }
  
  ao();
  sleep(.25);
  left_180();
  sleep(.75);
  open_claw_straight();
  measured_forward(30);

  measured_reverse(10);
  close_claw();
  right_180();
  wall_align();

  count = 0;
  while(!((analog(FRONT_LEFT) > FL_threshold ) 
	  && (analog(FRONT_RIGHT) > FR_threshold)
	  && (analog(REAR_LEFT) < RL_threshold)
	  && (analog(REAR_RIGHT) < RR_threshold)))
    {
      forward();

      if( digital(LEFT_BALL_SENSOR) || digital(RIGHT_BALL_SENSOR))
	{
	  close_claw();
	  ao();
	  break;
	}
      
      if(count == 10000)
	break;

      count++;
    }

  ao();
  close_claw();
  left_90();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  left_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();
}


void white_3_4()
{
  wall_align();
  measured_forward(3);
  right_90();
  wall_align();
  
  measured_forward(30);
  left_90();
  sleep(.25);
  wall_align();
  sleep(.25);
  open_claw_straight();
  sleep(.25);

  measured_forward(55);
  right_180();
  wall_align();
 
  measured_forward(24);
  open_left();

  if (digital(FRONT_LEFT_BUMPER_1) || digital(FRONT_LEFT_BUMPER_2))
    config = 3;
  else
    config = 4;
  
  close_left();
  
  if(config == 3)
    measured_forward(23);

  left_90();
  
  while(!(digital(FRONT_LEFT_BUMPER_1) 
	  || digital(FRONT_LEFT_BUMPER_2) 
	  || digital(FRONT_RIGHT_BUMPER_1)
	  || digital(FRONT_RIGHT_BUMPER_2)))
  {
    forward();
  }
  
  ao();
  measured_reverse(10);
  left_90();
  sleep(.50);
  open_claw_straight();
  measured_forward(40);
  wall_align();
}