/** Calculate how repusled the scanning robot is by a nearby object. */
  public double calcObjRepulseSpeed(double robotX, double robotY, double obsX, double obsY) {
    double speed = 0.0;
    double distance = Math.sqrt(Math.pow(robotX - obsX, 2) + Math.pow(robotY - obsY, 2));

    if (distance <= OBJ_DISTANCE) {
      speed = ((OBJ_DISTANCE - distance) / OBJ_DISTANCE) * MAX_SPEED;
    }

    return speed;
  }
  /** Linearly decay the speed of the robot when it nears the goal. */
  public double calcRobotSpeedLinear(double robotX, double robotY, double goalX, double goalY) {
    double speed = 0;

    double distance = Math.sqrt(Math.pow(robotX - goalX, 2) + Math.pow(robotY - goalY, 2));
    if (distance >= GOAL_DISTANCE) {
      speed = MAX_SPEED;
    } else {
      speed = (distance / GOAL_DISTANCE) * MAX_SPEED + 0.5;
    }

    return speed;
  }
 /**
  * Decay the speed "globally." The goal doens't have a range at which it starts to cause a speed
  * drop off. Instead, the entire screen has a scaled gradient.
  */
 public int calcRobotSpeedGlobalFields(double robotX, double robotY, double goalX, double goalY) {
   double distance = Math.sqrt(Math.pow(robotX - goalX, 2) + Math.pow(robotY - goalY, 2));
   return (int) ((distance / MAX_DISTANCE) * MAX_SPEED + 0.5);
 }
/**
 * Robocode robot that locates and navigates toward a goal while also avoiding obstacles that it
 * gets near.
 */
public class VectorFieldsMultMovObsAvoid extends AdvancedRobot {
  private static final int SCREEN_WIDTH = 600;
  private static final int SCREEN_HEIGHT = 600;
  private static final double MAX_DISTANCE =
      Math.sqrt(Math.pow(SCREEN_WIDTH, 2) + Math.pow(SCREEN_HEIGHT, 2));
  private static final int MAX_SPEED = 5;
  private static final int GOAL_DISTANCE = 50;
  private static final int OBJ_DISTANCE = 200;
  private static final String GOAL_NAME = "VectorFields.VectorFieldsMovingGoal*";

  private double goalX, goalY;
  private double obsX, obsY;
  private boolean foundGoal = false;
  private boolean foundObstacle = false;

  private Map<String, Enemy> obstacles;

  public void run() {
    setAllColors(Color.GREEN);
    setTurnRadarRight(Double.POSITIVE_INFINITY);

    double robotX, robotY;
    double robotHeading, angleToGoal, angleToObs;
    double adjustment;
    double obsAngle, obsAdjustment;
    double angleDiff;
    double speedToGoal, speedFromObs;

    Enemy temp;
    obstacles = new HashMap<String, Enemy>(10);

    while (true) {
      if (foundGoal) {
        robotX = getX();
        robotY = getY();
        goalX = obstacles.get(GOAL_NAME).x;
        goalY = obstacles.get(GOAL_NAME).y;

        // Adjust robocode's returned heading so that 0 aligns with the positive x-axis instead of
        // the positive y-axis.
        // Also make it so that positive angle indicates a counter clockwise rotation instead of the
        // clockwise style
        // returned by robocode.
        robotHeading = 360 - (getHeading() - 90);
        angleToGoal = Math.toDegrees(Math.atan2(goalY - robotY, goalX - robotX));
        if (angleToGoal < 0) {
          angleToGoal += 360;
        }

        adjustment = angleToGoal - robotHeading;
        adjustment = normalizeAngle(adjustment);
        speedToGoal = calcRobotSpeedLinear(robotX, robotY, goalX, goalY);

        // Calculate how the robot's heading and speed should be affected by objects that it has
        // located
        // as it explores the world.
        Iterator it = obstacles.entrySet().iterator();
        while (it.hasNext()) {
          System.out.println("Iterating through objects.");

          Map.Entry pairs = (Map.Entry) it.next();

          // If the current item in the Iterator isn't the goal.
          if (!pairs.getKey().equals(GOAL_NAME)) {
            temp = (Enemy) pairs.getValue();
            speedFromObs = calcObjRepulseSpeed(robotX, robotY, temp.x, temp.y);

            // If the robot is in range of the object to be affected by it's repulsion.
            if (speedFromObs != 0) {
              obsAngle = Math.toDegrees(Math.atan2(robotY - temp.y, robotX - temp.x));
              if (obsAngle < 0) obsAngle += 360;

              angleDiff = obsAngle - angleToGoal;
              angleDiff = normalizeAngle(angleDiff);
              adjustment += (angleDiff * (speedFromObs / speedToGoal));
              speedToGoal -= speedFromObs;
            }
          }

          // Was getting a null pointer exception when using this. The internet lied about its
          // usefulness.
          // it.remove(); // avoids a ConcurrentModificationException
        }

        adjustment = normalizeAngle(adjustment);
        setTurnLeft(adjustment);
        // ahead(speedToGoal);
        setAhead(speedToGoal);
      }

      execute();
    }
  }

  /**
   * When scanning a robot we need to add it to the collection of scanned objects so it can be used
   * later for updates to the bots movement.
   */
  public void onScannedRobot(ScannedRobotEvent e) {
    double targetBearing = getHeading() + e.getBearing();
    double tmpX = getX() + e.getDistance() * Math.sin(Math.toRadians(targetBearing));
    double tmpY = getY() + e.getDistance() * Math.cos(Math.toRadians(targetBearing));
    String name = e.getName();

    if (name.equals(GOAL_NAME)) {
      foundGoal = true;
    }

    obstacles.put(name, new Enemy(tmpX, tmpY, e.getBearing()));

    setTurnRadarRight(getRadarTurnRemaining());
  }

  /** Linearly decay the speed of the robot when it nears the goal. */
  public double calcRobotSpeedLinear(double robotX, double robotY, double goalX, double goalY) {
    double speed = 0;

    double distance = Math.sqrt(Math.pow(robotX - goalX, 2) + Math.pow(robotY - goalY, 2));
    if (distance >= GOAL_DISTANCE) {
      speed = MAX_SPEED;
    } else {
      speed = (distance / GOAL_DISTANCE) * MAX_SPEED + 0.5;
    }

    return speed;
  }

  /**
   * Decay the speed "globally." The goal doens't have a range at which it starts to cause a speed
   * drop off. Instead, the entire screen has a scaled gradient.
   */
  public int calcRobotSpeedGlobalFields(double robotX, double robotY, double goalX, double goalY) {
    double distance = Math.sqrt(Math.pow(robotX - goalX, 2) + Math.pow(robotY - goalY, 2));
    return (int) ((distance / MAX_DISTANCE) * MAX_SPEED + 0.5);
  }

  /**
   * Normalize an angle so it falls in the range -180 to 180 in the least efficient manner possible.
   */
  public double normalizeAngle(double angle) {
    if (angle <= -360) {
      angle += 360;
    } else if (angle >= 360) {
      angle -= 360;
    }

    if (angle < -180) {
      angle += 360;
    } else if (angle > 180) {
      angle -= 360;
    }

    return angle;
  }

  /** Calculate how repusled the scanning robot is by a nearby object. */
  public double calcObjRepulseSpeed(double robotX, double robotY, double obsX, double obsY) {
    double speed = 0.0;
    double distance = Math.sqrt(Math.pow(robotX - obsX, 2) + Math.pow(robotY - obsY, 2));

    if (distance <= OBJ_DISTANCE) {
      speed = ((OBJ_DISTANCE - distance) / OBJ_DISTANCE) * MAX_SPEED;
    }

    return speed;
  }
}