/* align.c */

/* wall id:
   0: opponent's end (short wall)
   1: opponent's start pos (long wall)
   2: our end (short_wall)
   3: our start pos (long_wall)

   return value:
   0: ok, aligned
   1: turn failed
   2: timeout
*/
/*
int align_line(long half_timeout) {
  int fl, fr, rl, rr;
  long t = mseconds();
  setup_drive(50);
  while((mseconds() - t) < half_timeout) {
    fl = cds_get_color(PORT_CDS_FRONT_LEFT, 1);
    fr = cds_get_color(PORT_CDS_FRONT_RIGHT, 1);
    rl = cds_get_color(PORT_CDS_REAR_LEFT, 1);
    rr = cds_get_color(PORT_CDS_REAR_RIGHT, 1);
    if((rr == our_color) && (rl == our_color)) {
      setup_drive(50);
      while((mseconds() - t) < half_timeout) 
	{
	  if(!(rr == our_color) && !(rl == our_color)) {
 	    stop();
	    nav_set_pos(36.0, nav_get_y_pos(), PI);
	    return 0;
	  }
	  else if((rr == our_color) && !(rl == our_color)) {
	    setup_turn(-1.0, 50);
	  }
	  else if(!(rr == our_color) && (rl == our_color)) {
	    setup_turn(1.0, 50);
	  }
	  msleep(10L);
	}
    }
    else if((rr == our_color) && !(rl == our_color)) {
      setup_turn(1.0, 50);
      while((mseconds() - t) < half_timeout)
	{
	  if(rl == our_color) {
 	    stop();
	    nav_set_pos(36.0, nav_get_y_pos(), PI);
	    return 0;
	  }
	  msleep(10L);
	}
      setup_turn(1.0, -100);
      while((mseconds() -t) < half_timeout)
	{
	  if(rl == our_color) {
 	    stop();
	    nav_set_pos(36.0, nav_get_y_pos(), PI);
	    return 0;
	  }
	  msleep(10L);
	}
    }
    else if(!(rr == our_color) && (rl == our_color)) {
      setup_turn(-1.0, 50);
      while((mseconds() -t) < half_timeout)
	{
	  if(rl == our_color) {
 	    stop();
	    nav_set_pos(36.0, nav_get_y_pos(), PI);
	    return 0;
	  }
	  msleep(10L);
	}
      setup_turn(-1.0, -100);
      while((mseconds() -t) < half_timeout)
	{
	  if(rl == our_color) {
 	    stop();
	    nav_set_pos(36.0, nav_get_y_pos(), PI);
	    return 0;
	  }
	  msleep(10L);
	}
    }
    msleep(100L);
  }
}   
*/
int align_wall(int wall_id, long timeout)
{
  float x0, y0, a0;
  int a = 0;
  long t = mseconds();
  /*  if (turn_to((float)wall_id * PI_2 + PI, 100, 1))
      return 1;*/

  while(mseconds() - t < timeout)
  {
    setup_drive(-70);
    
    while(mseconds() - t < timeout && !digital(PORT_TOUCH_REAR_RIGHT)
	  && !digital(PORT_TOUCH_REAR_LEFT))
      msleep(10L);

    stop();
    if (digital(PORT_TOUCH_REAR_RIGHT) && digital(PORT_TOUCH_REAR_LEFT))
    {
      nav_set_wall_pos(wall_id);
      if (a)
      {
	if (wall_id == 0 || wall_id == 2)
	  nav_set_pos(nav_get_x_pos(), y0, nav_get_angle());
	else
	  nav_set_pos(x0, nav_get_y_pos(), nav_get_angle());
      }

      return 0;
    }
    if (digital(PORT_TOUCH_REAR_LEFT))
    {
      if (a == 0)
      {
	nav_update();
	x0 = nav_get_x_pos();
	y0 = nav_get_y_pos();
	a0 = nav_get_angle();
	a = 1;
      }
      setup_turn(-1.0, -70);
      while(mseconds() - t < timeout && digital(PORT_TOUCH_REAR_RIGHT))
	msleep(10L);
    }
    else if (digital(PORT_TOUCH_REAR_RIGHT))
    {
      if (a == 0)
      {
	nav_update();
	x0 = nav_get_x_pos();
	y0 = nav_get_y_pos();
	a0 = nav_get_angle();
	a = 1;
      }
      setup_turn(1.0, 70);
      while(mseconds() - t < timeout && !digital(PORT_TOUCH_REAR_LEFT))
	msleep(10L);
    }
  }

  stop();

  /* timeout */
  if (a)
    nav_set_pos(x0, y0, a0);
  return 2;
}