/* move.c */

/* seconds/radians */
#define SERVO_TURN_DELAY (0.5/PI)

/* the turn function */

/* setup the servos and motors for a turn around pivot at speed
   positive speed means clockwise turn, negative is ccw
   setup_turn is responsible for orienting the wheels before
   changing the motor power */

int setup_turn(float pivot, int turn_speed)
{
  float fl_radius, fr_radius;
  float rl_radius, rr_radius;
  float weight;
  float fl_angle, fr_angle;
  float old_fl_angle, old_fr_angle;

  if (fabs(pivot)>1.0 && fabs(pivot)<3.0)
  {
#ifdef DEBUG
    printf("turn: invalid pivot radius %f\n", pivot);
#endif
    return 1;
  }

  rl_radius=pivot+1.0;
  rr_radius=pivot-1.0;

  fl_angle=atan(rl_radius*(1.0/ROBOT_LENGTH))-PI_2;
  if (pivot<-1.0) fl_angle+=PI;
  fr_angle=atan(rr_radius*(1.0/ROBOT_LENGTH))+PI_2;
  if (pivot>1.0) fr_angle-=PI;

  old_fl_angle = get_servo_pos(SERVO_LEFT);
  set_servo_pos(SERVO_LEFT, fl_angle);
  old_fr_angle = get_servo_pos(SERVO_RIGHT);
  set_servo_pos(SERVO_RIGHT, fr_angle);
  fl_angle = fabs(fl_angle - old_fl_angle);
  fr_angle = fabs(fr_angle - old_fr_angle);
  nav_update_lock();
  if (fl_angle != 0.0 || fr_angle != 0.0)
    nav_update_lock();
  if (fl_angle > fr_angle)
    sleep(fl_angle * SERVO_TURN_DELAY);
  else
    sleep(fr_angle * SERVO_TURN_DELAY);
  
  nav_update_lock();

  fl_radius=sqrt(rl_radius*rl_radius+ROBOT_LENGTH*ROBOT_LENGTH);
  fr_radius=sqrt(rr_radius*rr_radius+ROBOT_LENGTH*ROBOT_LENGTH);

  if (fl_radius > fr_radius)
    weight=(float)turn_speed*(1.0/fl_radius);
  else
    weight=(float)turn_speed*(1.0/fr_radius);

  if (fabs(pivot) > 1.0)
  {
    /* moving turn */

    set_motor_speed(MOTOR_FRONT_LEFT, (int)(weight*fl_radius));
    set_motor_speed(MOTOR_FRONT_RIGHT, (int)(weight*fr_radius));
    set_motor_speed(MOTOR_REAR_LEFT, (int)(weight*fabs(rl_radius)));
    set_motor_speed(MOTOR_REAR_RIGHT, (int)(weight*fabs(rr_radius)));
  }
  else
  {
    /* stationary turn */

    set_motor_speed(MOTOR_FRONT_LEFT, (int)(-weight*fl_radius));
    set_motor_speed(MOTOR_FRONT_RIGHT, (int)(weight*fr_radius));
    set_motor_speed(MOTOR_REAR_LEFT, (int)(-weight*rl_radius));
    set_motor_speed(MOTOR_REAR_RIGHT, (int)(-weight*rr_radius));
  }
  nav_update_lock();
  return 0;
}

/* make the robot drive (quasi-straight) */

int setup_drive(int speed)
{
  nav_update_lock();
  set_servo_pos(SERVO_LEFT, 0.0);
  set_servo_pos(SERVO_RIGHT, 0.0);

  set_motor_speed(MOTOR_FRONT_LEFT, speed);
  set_motor_speed(MOTOR_FRONT_RIGHT, speed);
  set_motor_speed(MOTOR_REAR_LEFT, speed);
  set_motor_speed(MOTOR_REAR_RIGHT, speed);

  return 0;
}

/* are the servos in position for a moving turn? */

int moving_turn()
{
  float l = get_servo_pos(SERVO_LEFT);
  float r = get_servo_pos(SERVO_RIGHT);

  if (l < 0.0 && r > 0.0)
    return 0;
  else
    return 1;
}

float get_front_wheel_angle()
{
  float l = get_servo_pos(SERVO_LEFT);
  float r = get_servo_pos(SERVO_RIGHT);

  if (l > 0.0)
    return l;
  else
    return r;
}

int set_front_wheel_angle(float angle, int speed)
{
  angle = restrict_diff_angle(angle);

  if (fabs(angle) > PI_2*0.5)
    return 1;

  if (angle > 0.0)
    setup_turn(-(1.0/qsin(angle)) * qcos(angle) * ROBOT_LENGTH - 1.0, speed);
  else if (angle < 0.0)
    setup_turn(-(1.0/qsin(angle)) * qcos(angle) * ROBOT_LENGTH + 1.0, speed);
  else
    setup_drive(speed);

  return 0;
}

float angular_delay_const = 30.0 / 130.0;
#define MIN_TURN_TO_DIFF_ANGLE = 1.0 / 180.0;
int turn_to(float angle, int speed, int b)
{
  float diff_angle;
  long t;

  stop();
  diff_angle = restrict_diff_angle(angle-nav_get_angle());
  if (fabs(diff_angle) < 2.0 * RAD_PER_DEG)
    return 0;
  else if (diff_angle > 0.0)
    setup_turn(0.0, speed);
  else if(diff_angle < 0.0)
    setup_turn(0.0, -speed);

  t = mseconds();
  while(fabs(restrict_diff_angle(angle-nav_get_angle())) >
	fabs(nav_get_angular_velocity()) * angular_delay_const)
  {
    if (b && mseconds()-t > 400L && fabs(nav_get_angular_velocity()) < PI/18.0)
    {
      stop();
      printf("turn_to blocked: w=%f\n", nav_get_angular_velocity());
      return 1;
    }
    nav_update_angle();
  }
  stop();
  return 0;
}

void stop()
{
  int i;
  nav_update_lock();

  for (i=0; i<4; i++)
  {
    int s=get_motor_speed(i);
    if (s>0) s=1;
    else if (s<0) s=-1;
    set_motor_speed(i, s);
  }
}