public boolean isStillInPenaltyDefence(Snapshot snapshot) {

    Coord ball = snapshot.getBall().getPosition();
    if ((ball == null) && snapshot.getOwnGoal().isLeftGoal())
      ball = new Coord(0.5, 0.6); // assume that ball is on penalty spot,
    // if
    // we cannot see it.
    if ((ball == null) && !snapshot.getOwnGoal().isLeftGoal())
      ball = new Coord(1.8, 0.6); // assume that ball is on penalty spot,
    // if
    // we cannot see it.

    double minX = 0;
    double maxX = 0.75;
    if (!snapshot.getOwnGoal().isLeftGoal()) {
      maxX = snapshot.getPitch().getMaxX();
      minX = maxX - 0.75;
    }

    if (ball.isEstimated()
        || (ball.getY() < snapshot.getOwnGoal().getMaxY())
            && (ball.getY() > snapshot.getOwnGoal().getMinY())
            && (ball.getX() > minX)
            && (ball.getX() < maxX)) {

      return true;
    }

    finished = true;
    return false;
  }
  @Override
  public void draw(Graphics g, Scaler s) {
    g.setColor(Color.WHITE);

    ArrayList<Coord> coords = new ArrayList<Coord>();
    for (double t = 0.0; t <= 1.0; t += 0.01) coords.add(pos(t));

    for (int i = 0; i + 1 < coords.size(); i++) {
      Coord c1 = coords.get(i), c2 = coords.get(i + 1);

      int x1, y1, x2, y2;
      x1 = (int) (s.m2PX(c1.getX()));
      y1 = (int) (s.m2PY(c1.getY()));
      x2 = (int) (s.m2PX(c2.getX()));
      y2 = (int) (s.m2PY(c2.getY()));
      g.drawLine(x1, y1, x2, y2);
    }

    g.setColor(Color.RED);
    for (Coord c : p) {
      int x, y, w = 4;
      x = (int) (s.m2PX(c.getX()) - (w / 2));
      y = (int) (s.m2PY(c.getY()) - (w / 2));
      g.fillRect(x, y, w, w);
    }
  }
  public MovementDirection getMovementDirection(Snapshot snapshot) {
    Robot opponent = snapshot.getOpponent();
    Robot ourRobot = snapshot.getBalle();
    Goal ourGoal = snapshot.getOwnGoal();
    Ball ball = snapshot.getBall();

    if ((opponent.getPosition() == null) || (ourRobot.getPosition() == null))
      return MovementDirection.NONE;

    Coord intersectionPoint = null;
    if ((ball.getPosition() != null) && (!ball.getPosition().isEstimated()))
      intersectionPoint =
          opponent.getBallKickLine(ball).getIntersect(ourRobot.getFacingLineBothWays());

    if (intersectionPoint == null)
      intersectionPoint = opponent.getFacingLine().getIntersect(ourRobot.getFacingLineBothWays());

    if (intersectionPoint == null) return MovementDirection.NONE;

    double yCoord = intersectionPoint.getY();
    yCoord = Math.max(yCoord, ourGoal.getMinY() + Globals.ROBOT_LENGTH / 3);
    yCoord = Math.min(yCoord, ourGoal.getMaxY() - Globals.ROBOT_LENGTH / 3);

    Coord targetPoint = new Coord(ourRobot.getPosition().getX(), yCoord);
    addDrawable(new Dot(targetPoint, Color.WHITE));

    boolean isUpward;
    Orientation ourOrientation = ourRobot.getOrientation();
    if ((ourOrientation.radians() >= 0) && (ourOrientation.radians() < Math.PI)) isUpward = true;
    else isUpward = false;

    // If we are already blocking the point stop
    if (ourRobot.containsCoord(targetPoint)) return MovementDirection.NONE;

    double diff = (targetPoint.getY() - ourRobot.getPosition().getY());
    if (diff > 0)
      if (isUpward) return MovementDirection.FORWARD;
      else return MovementDirection.BACKWARD;
    else if (isUpward) return MovementDirection.BACKWARD;
    else return MovementDirection.FORWARD;
  }
  @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);
      }
    }
  }