/* main.c */

/*
void speed_process()
{
  while(1)
  {
    printf("%f in/s\n", (float)nav_get_velocity()*INCHES_PER_PULSE/2.0);
    msleep(100L);
  }
} 

int test_turn(int s)
{
  float w;
  float a;
  setup_turn(0.0, 1);
  sleep(0.5);
  setup_turn(0.0, s);
  sleep(1.0);
  w = nav_get_angular_velocity();
  nav_update_angle();
  a = nav_get_angle();
  stop();
  sleep(1.0);
  nav_update_angle();
  printf("s: %d  w: %f  a: %f\n", s, w * 180.0/PI,
	 (nav_get_angle()-a) * 180.0/PI);
}
*/

/* 0: facing other side 
   1: facing ball
   2: facing own side
   3: facing out
   -1: cannot determine */

int main()
{
  int n, o;
  float s;

  pre_init();
  start_machine(PORT_CDS_CENTER);
  set_digital_out(6);
  post_init();

  sleep(0.5);
  
  o = -1; n = 0;
  while(o == -1 && n < 10)
  {
    o = find_orientation_led(1);
    n++;
    msleep(10L);
  }
  if (o == -1)
    o = find_orientation_ir();
  orient(o);
  turn_to(PI, 100, 1);

  n = 0;
  while (1)
  {
    if (n == 0)
      n = get_ball_kamikaze(100, 5000L);
    if (n == 0)
    {
      /* we have a ball */
      long t;

      /* clear the wall */
      if (nav_get_y_pos() < 0.0)
	turn_to(1.5 * PI_2, 100, 1);
      else
	turn_to(-1.5 * PI_2, 100, 1);
      drive_dist(100, 4.0, 1);

      /* setup the ball roll */
      move_to(26.0, -15.0, 100);
      turn_to(PI * 1.5, 100, 1);
      turn_to(PI * 2.2, 100, 1);

      /* roll on */
      setup_drive(-100);
      nav_reset_speedometer();
      t = mseconds();
      while(mseconds() - t < 3000L &&
	    nav_read_speedometer() < (int)(12.0 / INCHES_PER_PULSE))
	/* 5 inches */
	msleep(10L);
      if (mseconds() - t < 3000L)
      {
	set_arm_pos(0.0);
	set_motor_speed(MOTOR_REAR_CENTER, 100);
	msleep(100L);
	set_motor_speed(MOTOR_REAR_CENTER, 0);
	drive_dist(100, 8.0, 1);
      }
      else
      {
	/* over the edge */
	drive_dist(100, 8.0, 1);
	turn_to(PI, 100, 1);
	sleep(1.0);
	set_arm_pos(80.0 * RAD_PER_DEG);
	setup_drive(-70);
	t = mseconds();
	while(mseconds() - t < 7000L && nav_get_velocity() > 10.0)
	  msleep(10L);

	set_motor_speed(MOTOR_REAR_CENTER, 100);
	t = mseconds();
	while(mseconds() - t < 5000L && !digital(PORT_TOUCH_REAR_LEFT) &&
	      !digital(PORT_TOUCH_REAR_RIGHT))
	  msleep(10L);
	set_arm_pos(0.0);
	if (digital(PORT_TOUCH_REAR_LEFT) ||
	    digital(PORT_TOUCH_REAR_RIGHT))
	  align_wall(0, 4000L);
	drive_dist(100, 4.0, 1);
      }
      turn_to(PI_2, 100, 1);
      align_wall(3, 10000L);
      drive_dist(100, 4.0, 1);
    }
    move_to(25.0, -12.0, 100);

    turn_to(PI_2, 100, 1);
    n = find_ball(100, 2000L);

    if (n == 0)
      turn_to(ball_angle, 100, 1);
    else
    {
      move_to(25.0, 7.0, 100);
      turn_to(3.0*PI_2, 100, 1);
      n = find_ball(-100, 2000L);
      if (n == 0)
	turn_to(ball_angle, 100, 1);
    }
  }
}