@Override
  public void onStep(Controller controller, Snapshot snapshot) throws ConfusedException {

    if (finished || !isStillInPenaltyDefence(snapshot)) {
      super.onStep(controller, snapshot);
      return;
    }

    if (snapshot.getBalle().getPosition() == null) {
      return;
    }

    if ((firstSnapshot == null) && (snapshot.getBall().getPosition() != null)) {
      firstSnapshot = snapshot;
    }

    MovementDirection movementDirection = getMovementDirection(snapshot);
    if (movementDirection == MovementDirection.FORWARD) controller.setWheelSpeeds(SPEED, SPEED);
    else if (movementDirection == MovementDirection.BACKWARD)
      controller.setWheelSpeeds(-SPEED, -SPEED);
    else controller.stop();
  }
  @Override
  public void step(Controller controller, Snapshot snapshot) {
    drawables.clear();
    if (!isPossible(snapshot)) return;

    Pos opponent;
    if ((snapshot.getOpponent().getOrientation() == null)
        || (snapshot.getOpponent().getPosition() == null)) opponent = null;
    else
      opponent =
          new Pos(
              new Point(
                  snapshot.getOpponent().getPosition().getX(),
                  snapshot.getOpponent().getPosition().getY()),
              snapshot.getOpponent().getOrientation().radians());

    // Our pos
    Pos initPos =
        new Pos(
            new Point(
                snapshot.getBalle().getPosition().getX(), snapshot.getBalle().getPosition().getY()),
            snapshot.getBalle().getOrientation().radians());

    // Target pos
    Point targetLoc = null;
    Point targetLoc2 = new Point(target.getPosition().getX(), target.getPosition().getY());
    System.out.println(target.getPosition().getY());
    if (target.getPosition().getY() >= 0.6) {
      targetLoc = new Point(target.getPosition().getX(), target.getPosition().getY() - 0.2);
    } else {
      targetLoc = new Point(target.getPosition().getX(), target.getPosition().getY() + 0.2);
    }
    if (snapshot.getBalle().getPosition().dist(new Coord(targetLoc.getX(), targetLoc.getY()))
        < 0.1) {
      targetLoc = targetLoc2;
    }
    VelocityVec res = plann.update(initPos, opponent, targetLoc);
    if (!shouldSlowDownCloseToTarget()) {
      Vector newRes = res.mult(4);
      res = new VelocityVec(newRes.getX(), newRes.getY());
    }
    LOG.trace(
        "UNSCALED Left speed: "
            + Math.toDegrees(res.getLeft())
            + " right speed: "
            + Math.toDegrees(res.getRight()));
    double left, right;

    res = res.scale();

    left = Math.toDegrees(res.getLeft());
    right = Math.toDegrees(res.getRight());

    drawables.add(
        new Label(
            String.format("(%d, %d)", (int) left, (int) right),
            new Coord(1, Globals.PITCH_HEIGHT + 0.3),
            Color.BLACK));

    controller.setWheelSpeeds((int) left, (int) right);
  }
  @Override
  public void onStep(Controller controller, Snapshot snapshot) throws ConfusedException {

    if (snapshot.getBalle().getPosition() == null) return;

    // Make sure to reset the speeds if we haven't been dribbling for a
    // while
    long currentTime = System.currentTimeMillis();
    boolean facingOwnGoalSide = snapshot.getBalle().isFacingGoalHalf(snapshot.getOwnGoal());

    lastDribbled = currentTime;

    boolean facingGoal =
        snapshot.getBalle().getFacingLine().intersects(snapshot.getOpponentsGoal().getGoalLine());

    if (snapshot.getBall().getPosition() != null)
      facingGoal =
          facingGoal
              || snapshot
                  .getBalle()
                  .getBallKickLine(snapshot.getBall())
                  .intersects(snapshot.getOpponentsGoal().getGoalLine());

    if (currentSpeed <= 560) {
      currentSpeed += 20;
    }

    if (turnSpeed <= 150) {
      turnSpeed += 5;
    }

    double distanceToBall =
        snapshot.getBalle().getFrontSide().midpoint().dist(snapshot.getBall().getPosition());

    if (snapshot.getBall().getPosition().isEstimated()) distanceToBall = 0;

    boolean aboutToLoseBall = distanceToBall >= ABOUT_TO_LOSE_BALL_THRESHOLD;
    Color c = Color.BLACK;
    if (aboutToLoseBall) c = Color.PINK;

    Coord ourPos = snapshot.getBalle().getPosition();
    addDrawable(
        new Label(
            String.format("%.5f", distanceToBall), new Coord(ourPos.getX(), ourPos.getY()), c));

    int turnSpeedToUse = turnSpeed;

    boolean isLeftGoal = snapshot.getOpponentsGoal().isLeftGoal();

    double angle = snapshot.getBalle().getOrientation().radians();

    double threshold = Math.toRadians(5);

    boolean nearWall = snapshot.getBall().isNearWall(snapshot.getPitch());
    boolean wereNearWall = snapshot.getBalle().isNearWall(snapshot.getPitch(), SPINNING_DISTANCE);

    boolean closeToGoal =
        snapshot.getOpponentsGoal().getGoalLine().dist(snapshot.getBalle().getPosition())
            < SPINNING_DISTANCE;

    // Actually it might be helpful to turn when we're in this situation
    // close to our own goal
    closeToGoal =
        closeToGoal
            || snapshot.getOwnGoal().getGoalLine().dist(snapshot.getBalle().getPosition())
                < SPINNING_DISTANCE;
    // Turn twice as fast near walls
    if (nearWall) turnSpeedToUse *= 2;

    if ((!closeToGoal) && (nearWall) && wereNearWall) {
      Coord goalVector = snapshot.getOwnGoal().getGoalLine().midpoint().sub(ourPos);
      Orientation angleTowardsGoal = goalVector.orientation();

      // Always turn opposite from own goal
      boolean shouldTurnRight = !angleTowardsGoal.isFacingRight(0);

      // If we're facing wall
      if ((Math.abs(angle) <= FACING_WALL_THRESHOLD)
          || (Math.abs(angle - Math.PI / 2) <= FACING_WALL_THRESHOLD)
          || (Math.abs(angle - Math.PI) <= FACING_WALL_THRESHOLD)
          || (Math.abs(angle - 3 * Math.PI / 2) <= FACING_WALL_THRESHOLD)) {
        LOG.info("Spinning!!!");

        // If We are facing the bottom wall we should flip the spinning
        // directions
        if (Math.abs(angle - 3 * Math.PI / 2) <= FACING_WALL_THRESHOLD)
          shouldTurnRight = !shouldTurnRight;
        else if ((Math.abs(angle) <= FACING_WALL_THRESHOLD)
            || (Math.abs(angle - Math.PI) <= FACING_WALL_THRESHOLD)) {
          // If we're facing one of the walls with goals
          boolean facingLeftWall = (Math.abs(angle) <= FACING_WALL_THRESHOLD);
          if (facingLeftWall) {
            shouldTurnRight =
                snapshot.getOpponentsGoal().getGoalLine().midpoint().getY() > ourPos.getY();

            // Turn away from own goal
            if (snapshot.getOwnGoal().isLeftGoal()) shouldTurnRight = !shouldTurnRight;
          } else {
            shouldTurnRight =
                snapshot.getOpponentsGoal().getGoalLine().midpoint().getY() < ourPos.getY();

            // Turn away from own goal
            if (snapshot.getOwnGoal().isRightGoal()) shouldTurnRight = !shouldTurnRight;
          }
        }
        if (shouldTurnRight) spinRight(snapshot, controller, Globals.MAXIMUM_MOTOR_SPEED);
        else spinLeft(snapshot, controller, Globals.MAXIMUM_MOTOR_SPEED);

        return;
      }
    }

    if (isLeftGoal) {
      if (facingGoal) {
        controller.setWheelSpeeds(Globals.MAXIMUM_MOTOR_SPEED, Globals.MAXIMUM_MOTOR_SPEED);
      } else if ((!facingGoal) && (angle < Math.PI - threshold)) {
        controller.setWheelSpeeds(currentSpeed, currentSpeed + turnSpeedToUse);
      } else if ((!facingGoal) && (angle > Math.PI + threshold)) {
        controller.setWheelSpeeds(currentSpeed + turnSpeedToUse, currentSpeed);
      } else {
        controller.setWheelSpeeds(currentSpeed, currentSpeed);
      }
    } else {
      if (facingGoal) {
        controller.setWheelSpeeds(Globals.MAXIMUM_MOTOR_SPEED, Globals.MAXIMUM_MOTOR_SPEED);
      } else if ((!facingGoal) && (angle > threshold) && (angle < Math.PI)) {
        controller.setWheelSpeeds(currentSpeed + turnSpeedToUse, currentSpeed);
      } else if ((!facingGoal) && (angle < (2 * Math.PI) - threshold) && (angle > Math.PI)) {
        controller.setWheelSpeeds(currentSpeed, currentSpeed + turnSpeedToUse);
      } else {
        controller.setWheelSpeeds(currentSpeed, currentSpeed);
      }
    }
  }
 public void spinRight(Snapshot snapshot, Controller controller, int speed) {
   controller.setWheelSpeeds(speed, -speed);
   addDrawable(new Label("--->", snapshot.getBalle().getPosition(), Color.CYAN));
 }