/* navigation.c */

float nav_angle;
float nav_x_pos, nav_y_pos;

float nav_last_angle, nav_last_c, nav_last_s;
float nav_last_x_pos, nav_last_y_pos;
long nav_latest_update;

int nav_last_left_encoder, nav_last_right_encoder;
int nav_left_encoder[3], nav_right_encoder[3];
long nav_encoder_time[3];
int nav_next_encoder_index;
int nav_speedometer_zero;
int nav_process_lock;

/* api */

void nav_set_pos(float new_x, float new_y, float new_angle)
{
  nav_last_x_pos = nav_x_pos = new_x;
  nav_last_y_pos = nav_y_pos = new_y;
  nav_last_angle = nav_angle = restrict_angle(new_angle);
  nav_last_s = qsin(nav_last_angle);
  nav_last_c = qcos(nav_last_angle);
  reset_shaft();
}

void nav_set_wall_pos(int wall_id)
{
  nav_update();
  if (wall_id & 1)
  {
    nav_set_pos(nav_get_x_pos(), BOARD_MAX_Y * (float)(2 - wall_id),
		(float) wall_id * PI_2 + PI);
  }
  else
  {
    nav_set_pos(BOARD_MAX_X * (float)(1 - wall_id), nav_get_y_pos(),
		(float) wall_id * PI_2 + PI);
  }
}     

void nav_reset_speedometer()
{
  nav_speedometer_zero = 
    read_encoder(PORT_SHAFT_LEFT) + nav_last_left_encoder +
    read_encoder(PORT_SHAFT_RIGHT) + nav_last_right_encoder;
}

int nav_read_speedometer()
{
  return read_encoder(PORT_SHAFT_LEFT) + nav_last_left_encoder +
    read_encoder(PORT_SHAFT_RIGHT) + nav_last_right_encoder -
    nav_speedometer_zero;
}

float nav_get_x_pos()
{
  return nav_x_pos;
}

float nav_get_y_pos()
{
  return nav_y_pos;
}

float nav_get_angle()
{ 
  return nav_angle;
}

int init_nav()
{
  int i;

  enable_encoder(PORT_SHAFT_LEFT);
  enable_encoder(PORT_SHAFT_RIGHT);

  nav_last_left_encoder = nav_last_right_encoder = 0;
  for (i=0; i<3; i++)
  {
    nav_left_encoder[i] = nav_right_encoder[i] = 0;
    nav_encoder_time[i] = (long)(i-3);
  }

  nav_process_lock = 0;
  nav_next_encoder_index = 0;
  reset_shaft();
}

void nav_update_angle()
{
  int left = read_encoder(PORT_SHAFT_LEFT);
  int right = read_encoder(PORT_SHAFT_RIGHT);

  if (motor_speed[MOTOR_REAR_LEFT] < 0)
    left = -left;
  if (motor_speed[MOTOR_REAR_RIGHT] < 0)
    right = -right;

  nav_angle = restrict_angle(nav_last_angle+get_turn_angle(left, right));
}

/* returns velocity in ticks / second */
float nav_get_velocity()
{
  int i = nav_next_encoder_index - 1;
  int j;

  if (i < 0)
    i = 2;
  j = i-1;
  if (j < 0)
    j = 2;

  return (float)(nav_left_encoder[i] + nav_right_encoder[i] -
	  nav_left_encoder[j] - nav_right_encoder[j]) * 1000.0 /
    (float)(nav_encoder_time[i] - nav_encoder_time[j]);
}

/* angular velocity in radians per second */
float nav_get_angular_velocity_const = 1000.0 * INCHES_PER_PULSE/(HALF_RADIUS_IN_INCHES*2.0);
float nav_get_angular_velocity()
{
  int i = nav_next_encoder_index - 1;
  int j;
  int l, r;

  if (i < 0)
    i = 2;
  j = i-1;
  if (j < 0)
    j = 2;

  l = nav_left_encoder[i] - nav_left_encoder[j];
  r = nav_right_encoder[i] - nav_right_encoder[j];
  if (get_motor_speed(MOTOR_REAR_LEFT) < 0)
    l = -l;
  if (get_motor_speed(MOTOR_REAR_RIGHT) < 0)
    r = -r;

  return (float)(r-l) * nav_get_angular_velocity_const /
    (float)(nav_encoder_time[i] - nav_encoder_time[j]);
}

void nav_update()
{
  _nav_update(0);
}

/* call whenever actuators change */
void nav_update_lock()
{
  _nav_update(1);
}

/* internal functions */

void nav_process()
{
  while(1)
  {
    nav_process_lock++;
    while(nav_process_lock != 1)
      defer();
    nav_left_encoder[nav_next_encoder_index] =
      read_encoder(PORT_SHAFT_LEFT) + nav_last_left_encoder;
    nav_right_encoder[nav_next_encoder_index] =
      read_encoder(PORT_SHAFT_RIGHT) + nav_last_right_encoder;
    nav_encoder_time[nav_next_encoder_index] = mseconds();
    if (nav_next_encoder_index == 2)
      nav_next_encoder_index = 0;
    else
      nav_next_encoder_index++;
    nav_process_lock--;
    msleep(50L);
  }
}
    
void reset_shaft()
{
  while(1)
  {
    hog_processor();
    if (nav_process_lock)
      defer();
    else
    {
      nav_last_left_encoder += read_encoder(PORT_SHAFT_LEFT);
      reset_encoder(PORT_SHAFT_LEFT);
      nav_last_right_encoder += read_encoder(PORT_SHAFT_RIGHT);
      reset_encoder(PORT_SHAFT_RIGHT);
      return;
    }
  }
}

float get_turn_angle_const = INCHES_PER_PULSE/(HALF_RADIUS_IN_INCHES*2.0);
float get_turn_angle(int l, int r)
{
  return ((float)(r-l))*get_turn_angle_const;
}

/* return value in inches */
float get_pivot_point_const = (HALF_RADIUS_IN_INCHES*2.0);
float get_pivot_point(int l, int r)
{
  if (abs(l) >= abs(r) && l!=r)
    return -HALF_RADIUS_IN_INCHES -
      (float)l * (1.0 / (float)(r-l)) * get_pivot_point_const;
  else if (abs(r) > abs(l))
    return HALF_RADIUS_IN_INCHES - 
      (float)r * (1.0 / (float)(r-l)) * get_pivot_point_const;
}

void _nav_update(int lock)
{
  float a, pivot;
  float dx, dy;

  int s;
  int left = read_encoder(PORT_SHAFT_LEFT);
  int right = read_encoder(PORT_SHAFT_RIGHT);

  if (lock)
    reset_shaft();

  s = get_motor_speed(MOTOR_REAR_LEFT);
  if (s < 0)
    left = -left;
  else if (s == 0)
    left = 0;
  s = get_motor_speed(MOTOR_REAR_RIGHT);
  if (s < 0)
    right = -right;
  else if (s == 0)
    right = 0;

  a = get_turn_angle(left, right);
  if (a != 0.0)
  {
    a = restrict_angle(a);
    pivot = get_pivot_point(left, right);
    dx = -pivot * qsin(a);
    dy = -pivot * (1.0 - qcos(a));
  }
  else
  {
    dx = (float)left*INCHES_PER_PULSE;
    dy = 0.0;
  }
  
  if (lock)
  {
    
    nav_last_x_pos += dx * nav_last_c - dy * nav_last_s;
    nav_last_y_pos += dx * nav_last_s + dy * nav_last_c;
    nav_last_angle = restrict_angle(nav_last_angle+a);
    nav_last_s = qsin(nav_last_angle);
    nav_last_c = qcos(nav_last_angle);
    nav_x_pos = nav_last_x_pos;
    nav_y_pos = nav_last_y_pos;
    nav_angle = nav_last_angle;
  }
  else
  {
    nav_x_pos = nav_last_x_pos + dx * nav_last_c - dy * nav_last_s;
    nav_y_pos = nav_last_y_pos + dx * nav_last_s + dy * nav_last_c;
    nav_angle = restrict_angle(nav_last_angle+a);
  }

  nav_latest_update=mseconds();
}