/* drive.c */

/* drives a fixed distance STRAIGHT and then stops */

int drive_dist(int speed, float distance, int handle_stop_error) 
{
  if(fabs(get_front_wheel_angle()) > (5.0*RAD_PER_DEG)) { 
    setup_drive(1);
    sleep(0.5);
  }
  if(drive(speed, distance, handle_stop_error) == 0) {
     stop();
     return 0;
  }
  else {
    return 1;
  }
}

/* drives a fixed distance STRAIGHT, but continues WITHOUT stopping */
int drive(int speed, float distance, int handle_stop_error)
{
  /* Set the count that needs to be reached before stoping */
  int count=(int)((2.0*distance)/INCHES_PER_PULSE);

  /* Record current angle and calculate line to travel */
  float angle = nav_get_angle();
  float a = -sin(angle);
  float b = cos(angle);
  float c = a*nav_get_x_pos() + b*nav_get_y_pos();

  /* Intialize frong wheel angle */
  float front_wheel_angle = 0.0;

  float offset;
  float future_angle;
  float delay_const = 0.215;

  /* Initialize initial stop checks */
  int handle_stop = 0;

  setup_drive(speed);
  nav_reset_speedometer();
  while(count >= nav_read_speedometer() + drive_delay(nav_get_velocity()))
  {
    /* updated navigation values */
    nav_update();
    
    /* check for blocked motion */
    if((nav_get_velocity() < 50.0) &&
       handle_stop)
    { 
      stop();
      return 1;
    }
    /* adjust motor speed to ensure straightness */
    offset = b*nav_get_y_pos() + a*nav_get_x_pos() - c;
    if(offset > 0.25) {
      if(front_wheel_angle > - PI/60.0)
	front_wheel_angle = front_wheel_angle - PI/180.0;
    }
    else if(offset < -0.25) {
      if(front_wheel_angle < PI/60.0)
	front_wheel_angle = front_wheel_angle + PI/180.0;
      
    }
    else {
      front_wheel_angle = -0.5*front_wheel_angle;
    }

      /*      future_angle = nav_get_angle() + nav_get_angular_velocity()*delay_const;
      if(future_angle > angle)
	front_wheel_angle = front
	+ nav_get_angular_velocity()*delay_const = 0.0)
	front_wheel_angle = 0.8*front_wheel_angle;*/

    if((handle_stop == 0) &&
       (nav_get_velocity() != 0.0)) {
      handle_stop = handle_stop_error;
    }

    set_front_wheel_angle(front_wheel_angle, speed);
    sleep(0.2);
  }
  nav_update();
  printf("x:%f y:%f a:%f", nav_get_x_pos(), nav_get_y_pos(), nav_get_angle());
  return 0;
}

int drive_delay(float velocity) {
  return (int) (0.15 * velocity);
}

int decrement_speed(int speed)
{
  if(speed < 0)
    return speed+1;
  else if(speed > 0)
    return speed-1;
  else
    return 0;
}

int increment_speed(int speed)
{
  if(speed < 0) {
    speed--;
    if(speed < -100)
      return -100;
    else
      return speed;
  }
  else if(speed > 0) {
    speed++;
    if(speed > 100)
      return 100;
    else
      return speed;
  }
  else
    return 0;
}