/* ball.c */

#define BALL_EDGE_NONE 0
#define BALL_EDGE_RISING 1
#define BALL_EDGE_FALLING 2

#define DISTANCE_SENSOR_DELAY_MS 40L

float ball_x, ball_y;

float ball_angle;
int ball_dist;

int ball_edge(int d0, int d1, int d2)
{
  /* if the second derivative is positive and the
     actual measurements differ by more than 4 inches
     we found a falling edge */
  if (d0 - d1 - d1 + d2 > 0 && d0 - d2 > 40)
    return BALL_EDGE_FALLING;
  else if (d0 - d1 - d1 + d2 < 0 && d0 - d2 < 40)
    return BALL_EDGE_RISING;
  else
    return BALL_EDGE_NONE;
}

#define BALL_FOUND 0
#define BALL_UNKNOWN 1
#define BALL_TIMEOUT 2

/* returns 0 for ball found
   1 for unknown non-ball object found
   2 for timeout */
   
int find_ball(int speed, long timeout)
{
  int dist_a[3], dist_b[3], dist, n;
  int found = 0;
  int port_a, port_b;
  int val_a, val_b;
  long t = mseconds();

  set_arm_pos(0.0);

  while(!found && mseconds()-t < timeout)
  {
    setup_turn(0.0, speed);
    if (speed > 0)
    {
      port_a = PORT_DISTANCE_REAR_RIGHT;
      port_b = PORT_DISTANCE_REAR_LEFT;
    }
    else
    {
      port_a = PORT_DISTANCE_REAR_LEFT;
      port_b = PORT_DISTANCE_REAR_RIGHT;
    }

    dist_a[0]=dist_a[1]=distance(analog(port_a));
    dist_b[0]=dist_b[1]=distance(analog(port_b));
    while(found < 2 && mseconds()-t < timeout)
    {
      long dt;
      hog_processor();
      dt = mseconds();
      val_a = analog(port_a);
      val_b = analog(port_b);
      nav_update_angle();
      dt = mseconds() - dt;
      defer();
      

      dist_a[2]=dist_a[1];
      dist_a[1]=dist_a[0];
      dist_a[0]=distance(val_a);

      dist_b[2]=dist_b[1];
      dist_b[1]=dist_b[0];
      dist_b[0]=distance(val_b);

      if (found == 0 &&
	  ball_edge(dist_a[0], dist_a[1], dist_a[2]) == BALL_EDGE_RISING)
      {
	/* slow down and start looking for falling edge */
	found = 1;
	ball_dist = dist_a[0];
      }
      else if (found == 1 &&
	       ball_edge(dist_a[0], dist_a[1], dist_a[2]) == BALL_EDGE_FALLING)
      {
	ball_angle = nav_get_angle() - nav_get_angular_velocity() * 0.001 *
	  (float)(DISTANCE_SENSOR_DELAY_MS + dt);

	/* we have a ball */
	stop();

	/* compute the actual coordinates of
	   the would-be center of the ball */

	ball_x = nav_get_x_pos() - qcos(ball_angle) * (float)(ball_dist + 25);
	ball_y = nav_get_y_pos() - qsin(ball_angle) * (float)(ball_dist + 25);

	if (verify_ball())
	  return 0;
	else
	  return 1;
      }
      else if (found == 1 && n < 100)
      {
	/* sample ball distance */
	if (ball_dist > dist_a[0]) ball_dist = dist_a[0];
      }
      else if (found == 0 &&
	       ball_edge(dist_b[0], dist_b[1], dist_b[2]) == BALL_EDGE_FALLING)
      {
	/* we are going the wrong way... */
	stop();
	found = 4;
	speed = -speed;
      }
    }
    if (found == 4)
      found = 0;
  }
  if (!found)
  {
    stop();
    return 2;
  }
  else
    return found-2;
}

/* return value:
   0: has ball
   1: grab failed
   2: run into an obstacle
   3: timeout

   note that the timeout parameter does not include turn time */
/*
int get_ball_const = (int)(4.0 / INCHES_PER_PULSE);
int get_ball(int speed, long timeout)
{
  long t;
  int dist;
  int n;
  float x, y, a, v, w;

  if(turn_to(ball_angle, speed, 1))
    return 2;

  t = mseconds();
  
  setup_drive(1);
  set_arm_pos(60.0 * RAD_PER_DEG);
  dist = (int)((float)(ball_dist - 20) * (2.0 / (10.0 * INCHES_PER_PULSE)));

  nav_reset_speedometer();
  setup_drive(-speed);

  while(mseconds() - t < timeout && nav_read_speedometer() <
	dist - drive_delay(nav_get_velocity()) - get_ball_const)
  {
    int l = analog(PORT_DISTANCE_REAR_LEFT);
    int r = analog(PORT_DISTANCE_REAR_RIGHT);
    l = distance(l);
    r = distance(r);

    if (l > r + 20)
      set_front_wheel_angle(-PI/10.0, -speed);
    else if (r > l + 20)
      set_front_wheel_angle(PI/10.0, -speed);
    else
      set_front_wheel_angle(0.0, -speed);
    msleep(10L);
  }
  if (mseconds() - t >= timeout)
  {
    set_arm_pos(0.0);
    stop();
    return 3;
  }
  while(mseconds() - t < timeout && nav_read_speedometer() < dist -
	drive_delay(nav_get_velocity()))
    msleep(10L);

  if (mseconds() - t >= timeout)
  {
    set_arm_pos(0.0);
    stop();
    return 3;
  }
  t = mseconds();
  nav_update();
  set_arm_pos(110.0*PI/180.0);
  x = nav_get_x_pos();
  y = nav_get_y_pos();
  a = nav_get_angle();
  v = nav_get_velocity() * 0.5 * INCHES_PER_PULSE;
  w = nav_get_angular_velocity();
  
  n = 0;
  while(n < 20 && get_arm_pot_angle() < 100.0 * PI / 180.0)
  {
    msleep(10L);
    n++;
  }
  stop();
  t = mseconds() - t;
  nav_set_pos(x - v * 0.001 * (float)t * qcos(a),
	      y - v * 0.001 * (float)t * qsin(a),
	      a + w * 0.001 * (float)t - PI / 18.0);
  if (get_arm_pot_angle() > 90.0 * PI / 180.0)
    return 0;
  else
  {
    set_arm_pos(0.0);
    return 1;
  }
}*/

/* return value:
   0: has ball
   1: grab failed
   2: run into an obstacle
   3: timeout

   note that the timeout parameter does not include turn time */

int get_ball_kamikaze(int speed, long timeout)
{
  long t;
  int n;
  float x, y, a, v, w;

  t = mseconds();
  
  setup_drive(1);
  set_arm_pos(60.0 * RAD_PER_DEG);

  /* center switch is the other way around */
  while(mseconds() - t < timeout && digital(PORT_TOUCH_REAR_CENTER) &&
	!digital(PORT_TOUCH_REAR_LEFT) && !digital(PORT_TOUCH_REAR_RIGHT))
  {
    int l = analog(PORT_DISTANCE_REAR_LEFT);
    int r = analog(PORT_DISTANCE_REAR_RIGHT);
    l = distance(l);
    r = distance(r);

    if (l > r + 30)
      set_front_wheel_angle(-PI/18.0, -speed);
    else if (r > l + 30)
      set_front_wheel_angle(PI/18.0, -speed);
    else
      set_front_wheel_angle(0.0, -speed);
    defer();
  }
  if (mseconds() - t >= timeout)
  {
    set_arm_pos(0.0);
    stop();
    return 3;
  }
  if (digital(PORT_TOUCH_REAR_LEFT) &&
      digital(PORT_TOUCH_REAR_RIGHT))
  {
    set_arm_pos(0.0);
    stop();
    return 2;
  }

  /* assume we have the ball */
  t = mseconds();
  nav_update();
  set_arm_pos(110.0*PI/180.0);
  x = nav_get_x_pos();
  y = nav_get_y_pos();
  a = nav_get_angle();
  v = nav_get_velocity() * 0.5 * INCHES_PER_PULSE;
  w = nav_get_angular_velocity();
  
  n = 0;
  while(n < 20 && get_arm_pot_angle() < 100.0 * PI / 180.0)
  {
    msleep(10L);
    n++;
  }
  stop();
  t = mseconds() - t;
  nav_set_pos(x - v * 0.001 * (float)t * qcos(a),
	      y - v * 0.001 * (float)t * qsin(a),
	      a + w * 0.001 * (float)t - PI / 18.0);
  if (get_arm_pot_angle() > 90.0 * PI / 180.0)
    return 0;
  else
  {
    set_arm_pos(0.0);
    return 1;
  }
}
/*
int get_ball_straight(int speed, long timeout)
{
  int n;
  long t = mseconds();
  if(turn_to(ball_angle, speed, 1))
    return 2;
  set_arm_pos(60*RAD_PER_DEG);
  if(drive(speed, ball_dist, 1))
    return 1;
  set_arm_pos(110*RAD_PER_DEG);
  while(n < 20 && get_arm_pot_angle() < 100.0 * PI / 180.0)
  {
    msleep(10L);
    n++;
  }
  stop();
  if (get_arm_pot_angle() > 90.0 * PI / 180.0)
    return 0;
  else
  {
    set_arm_pos(0.0);
    return 1;
  }
}
*/

int verify_ball()
{
  if (fabs(ball_x - BOARD_LINE_X) < 8.0)
    return 0;
  else
    return 1;
}