import java.lang.Math;

/** Helps the Rovers with trig, geometery, and conversion. */
public class RoboMath {

  public static final int ROBO_RADIUS = 400;
  public static final int MOVING_ROBO_RADIUS = 200;

  /** Converts degrees to radians */
  public static double toRadians (double degrees) {
    return (2*Math.PI*degrees)/360.0;
  }

  /** Returns the distance between two poses. */
  public static double distance (Pose p1, Pose p2) {
    return Math.sqrt(Math.pow(p1.x-p2.x,2) + Math.pow(p1.y-p2.y,2));
  }

  /** Returns whether one heading is closer to the left or right.
      Used to decide which way to turn in facing a certain heading. */
    public static boolean leftOf (int h1, int h2) {
	if (h1 > h2) {
	    return (h1-h2 < h2+(360-h1));
	}
	else {
	    return (h2-h1 < h1+(360-h2));
	}
    }

  /** Used in conjunction with the above to decide which way to turn
      to face a certain heading and how many degrees away it is. */
    public static int degreesAway (int h1, int h2) {
	h1 = adjustHeading(h1);
	h2 = adjustHeading(h2);
	int mult = leftOf(h1,h2) ? -1 : 1;
	return mult*magnitudeAway(h1,h2);
    }

  /** used to decide how far away (in degrees) one heading is from
      another, regardless of direction. */
    public static int magnitudeAway (int h1, int h2) {
	int max = Math.max(h1, h2);
	int min = Math.min(h1, h2);
	return Math.min(max-min, min+(360-max));
    }

  /** converts negative headings to positive. */
    public static int adjustHeading (int heading) {
	return (heading < 0) ? heading+360 : heading;
    }

  /** checks to see if two poses may be the same robot. */
  public static boolean isRobot (Pose p1, Pose p2, int radius) {
    double dist = distance(p1,p2);
    return dist <= radius;
  }

}