/* move_to.c */

/* Handles all movements of the robot between points. */

int move_to(float x, float y, int move_speed)
{
  float delta_x, delta_y;
  float angle;
  float distance;

  nav_update();

  delta_x = x - nav_get_x_pos();
  delta_y = y - nav_get_y_pos();

  if(delta_x == 0.0) {
    /* Avoid divide by zero error */
    if(delta_y > 0.0)
      angle = PI_2;
    else
      angle = -PI_2;
  }
  else {
    angle = atan(delta_y/delta_x);
    if(delta_x < 0.0)
      angle = angle + PI;
    if(move_speed < 0)
      angle = angle + PI;
  }
  
  distance = sqrt(delta_x*delta_x + delta_y*delta_y);
  
  /*printf("a:%f d:%f", angle, distance);*/

  if(turn_to(angle, abs(move_speed), 1) == 1) {
    return 1;
  }
  return drive_dist(move_speed, distance, 1);
}