package fch;

import robocode.Robot;

/**
 * A Robot object with the ability to move to a specified point.
 * @author Christopher Foo
 *
 */
public abstract class MovingRobot extends Robot{

  /**
   * Moves the robot to the specified point.
   * @param x The x-coordinate of the point to move to.
   * @param y The y-coordinate of the point to move to.
   */
  public void moveToPoint(double x, double y){

    while(Math.round(getX()) != Math.round(x) || Math.round(getY()) != Math.round(y)){

      //Account for the robot's width to prevent attempts to send the robot to an unreachable point.
      if(x < getWidth()/2){
        x = getWidth()/2;
      }
      else if(x > getBattleFieldWidth()-getWidth()/2){
        x = getBattleFieldWidth()-getWidth()/2;
      }

      //Account for the robot's height to prevent attempts to sent the robot to an unreachable
      //point.
      if(y < getHeight()/2){
        y = getHeight()/2;
      }
      else if(y > getBattleFieldHeight()-getHeight()/2){
        y = getBattleFieldHeight()-getHeight()/2;
      }

      moveToPointAttempt(x, y);
    }
  }

  /**
   * Attempts to move the robot to the specified point.
   * @param x The x-coordinate of the point to move to.
   * @param y The y-coordinate of the point to move to.
   */
  public void moveToPointAttempt(double x, double y){
    double currentHeading = getHeading(); //The robot's current heading.
    double desiredHeading; //The heading to the point to move to.
    double headingDifference; //The difference between the current and the desired headings.
    double currentX = getX(); //The robot's current x position.
    double currentY = getY(); //The robot's current y position.

    //Comparison of the robot's current x position to the desired x position.
    //>0 if current is larger
    // 0 if equal
    //<0 if smaller
    int compareX = ((Double) currentX).compareTo(x);

    //Comparison of the robot's current y position
    //to the desired y position.
    //>0 if current is larger
    // 0 if equal
    //<0 if smaller
    int compareY = ((Double) currentY).compareTo(y);

    if(compareY == 0){

      //On Point
      if(compareX == 0){
        return;
      }

      //East of Point
      else if(compareX > 0){
        desiredHeading = 270;
      }

      //West of Point
      else{
        desiredHeading = 90;
      }
    }
    else if(compareY > 0){

      //Directly North of Point
      if(compareX == 0){
        desiredHeading = 180;
      }

      //North-East of Point
      else if(compareX > 0){
        desiredHeading = Math.toDegrees(Math.abs(Math.atan((currentX-x)/(currentY-y))))
                         + 180;
      }

      //North-West of Point
      else {
        desiredHeading = 180
                         - Math.toDegrees(Math.abs(Math.atan((x-currentX)/(currentY-y))));

      }
    }

    //Else compareY must be < 0
    else {

      //Directly South of Point
      if(compareX == 0){
        desiredHeading = 180;
      }

      //South-East of Point
      else if(compareX > 0){
        desiredHeading = Math.toDegrees(Math.abs(Math.atan((y-currentY)/(currentX-x))))
                         + 270;
      }

      //South-West of Point
      else{
        desiredHeading = 90
                         - Math.toDegrees(Math.abs(Math.atan((y-currentY)/(x-currentX))));
      }
    }

    //Negative if desiredHeading is more clockwise
    //Positive if desiredHeading is more counter-clockwise
    //Absolute Value = difference in degree angles
    headingDifference = currentHeading - desiredHeading;

    //Would be large counter-clockwise turn so make smaller clockwise turn
    if(headingDifference > 180){
      turnRight(360-headingDifference);
    }

    //Would be large clockwise turn so make smaller counter-clockwise turn
    else if(headingDifference < -180){
      //headingDifference is negative so add instead of subtract to get turning angle
      turnLeft(360+headingDifference);
    }

    else {
      turnLeft(headingDifference);
    }

    ahead(Math.hypot(currentX-x, currentY-y));
  }
}