/**
  * This method "juggles" and helps position the block the robot wants to pick up. It will shift
  * left and right then close its arms to straighten the block so the bottom ultrasonic sensor
  * faces directly at the block.
  */
 private void juggle() {
   lifter.halfClose();
   // Shifting left and right
   dashboard.turn(-15);
   dashboard.turn(30);
   dashboard.turn(-15);
   // Straightening the block
   lifter.closeClaw();
   lifter.openClaw();
 }
  /**
   * This recursive method is responsible for finding the blue styrofoam block and returning it to
   * the green/red zone. The search and detect process will begin when the robot has reached the
   * green/red zone.
   *
   * @param x The x position of where the robot will start the search for the blue styrofoam block
   * @param y The y position of where the robot will start search for the blue styrofoam block
   */
  public void doSearchAndDetect(double x, double y) { // Searching inside the green/red zone
    if (firstTry) {
      odox = odometer.getX();
      odoy = odometer.getY();
      odotheta = odometer.getTheta();
      dashboard.turnLeft(40);
      try {
        Thread.sleep(750);
      } catch (Exception e) {
      }
    } else if (Math.abs(odox - odometer.getX()) > 15 && Math.abs(odoy - odometer.getY()) > 15) {
      odox = odometer.getX();
      odoy = odometer.getY();
      odotheta = odometer.getTheta();
      dashboard.turnLeft(40);
      try {
        Thread.sleep(750);
      } catch (Exception e) {
      }
    }
    firstTry = false;
    // Initializing the heading angle when a robot detects something 35 cm away
    detectionAngle = odometer.getTheta();
    Point greenZone = new Point((int) x, (int) y); // Remembering green zone
    dashboard.turnLeft(70); // Beginning the 360 degrees scan
    boolean clawIsDown = false;

    // The boolean found never becomes true so the robot will keep spinning until
    // it finds something then moves to a different zone or it will complete 1 full
    // revolution from its starting angle then move to a different zone.
    while (!found) {
      // Condition for the robot to move to a different area to search since it has
      // completed one full revolution
      if (Math.abs(odox - odometer.getX()) < 10
          && Math.abs(odoy - odometer.getY()) < 10
          && Math.abs(odotheta - odometer.getTheta())
              < 5) { // Case where no objects in search radius
        if (directionCounter
            == 1) { // First time around, go to a point 15 cm south west of the green zone bottom
                    // left corner
          directionCounter++;
          navigation.travelTo(greenZone.getX() - 15, greenZone.getY() - 15, false);
        } else if (directionCounter
            == 2) { // Second time around, go below the middle of the green zone
          directionCounter++;
          navigation.travelTo(greenZone.getX() + 30, greenZone.getY() - 15, false);
        } else if (directionCounter == 3) { // Third time, go north easy of of the green zone
          directionCounter++;
          navigation.travelTo(greenZone.getX() + 30, greenZone.getY() + 45, false);

        } else if (directionCounter
            == 4) { // Last time around, go to the middle of the playing field
          directionCounter = 1;
          navigation.travelTo(180, 180, false);
        }
        // After traveling to the new scan zone, perform the search and scan
        doSearchAndDetect(greenZone.getX(), greenZone.getY());
      } else {
        // If the ultrasonic sensor senses something within 35 centimeters, go to the object
        if (usLow.getFilteredDistance() <= 35
            && Math.abs(odometer.getTheta() - badHeading) > 30) { // Range to go check out object
          detectionAngle = odometer.getTheta(); // Saving heading angle when the object is detected
          blockAngle = odometer.getTheta();
          previousDistance = usLow.getFilteredDistance();
          navigation.turnTo(
              detectionAngle
                  - 30.0); // Turning to past where object was detected so it can analyze the full
                           // block
          Stopwatch stopwatch = new Stopwatch();
          // Scanning whole object for 3 seconds to find shortest point of the object to robot
          while (stopwatch.elapsed() < (3 * Math.pow(10, 3))) {
            dashboard.turnRight(
                70); // Turning to the right to analyze object since it overturned initially
            if (usLow.getFilteredDistance() < previousDistance) {
              dashboard.stop();
              previousDistance = usLow.getFilteredDistance();
              blockAngle = odometer.getTheta();
            }
          }

          dashboard.stop();
          navigation.turnTo(blockAngle); // Turn to block
          // Remembering tacho count in case object is a wooden block
          // so it will back up to exactly the same spot
          originalTacho = dashboard.getRightTacho();
          Stopwatch watch = new Stopwatch();
          while (usLow.getFilteredDistance() >= 10) {
            // If robot hasn't reached object in 8 seconds, most likely nothing there and false
            // positive
            if (watch.elapsed() > 9 * Math.pow(10, 3)) {
              badHeading = odometer.getTheta();
              newTacho = dashboard.getRightTacho();
              dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
              dashboard.stop();
              doSearchAndDetect(greenZone.getX(), greenZone.getY());
            }
            // Travel to the object by going forward
            dashboard.goForward(200);
          }
          // Making a check for a wooden block by using the top ultrasonic sensor
          if (usHigh.getFilteredDistance()
              < 25) { // Brown Block so back up distance traveled to the block
            dashboard.stop();
            badHeading =
                odometer
                    .getTheta(); // Saving the angle of the wooden block so it doesn't go back when
                                 // it sees it again
            newTacho = dashboard.getRightTacho();
            dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
            dashboard.stop();
            doSearchAndDetect(greenZone.getX(), greenZone.getY());
          } else { // Go forward a bit more to get closer to the styrofoam block
            for (int i = 0; i < 200; i++) {
              dashboard.goForward(70);
            }
          }
          // 2nd check for brown block to make sure when closer to object
          if (usHigh.getFilteredDistance() < 25) {
            dashboard.stop();
            badHeading = odometer.getTheta();
            newTacho = dashboard.getRightTacho();
            dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
            dashboard.stop();
            doSearchAndDetect(greenZone.getX(), greenZone.getY());
          } else { // Return to dropzone then drop off and re-do scan
            newTacho = dashboard.getRightTacho();
            // If doing search but the block is in the green/red zone, don't pick it up again
            if ((odometer.getX() > greenZone.getX() && odometer.getY() > greenZone.getY())
                && (odometer.getX() < greenZone.getX() + 30
                    && odometer.getY() < greenZone.getY() + 30)) {
              badHeading = odometer.getTheta();
              dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
              dashboard.stop();
              doSearchAndDetect(greenZone.getX(), greenZone.getY());
            }
            dashboard.stop();
            // Reached a good distance from the styrofoam block so back up
            // and get ready for the juggle.
            Stopwatch secondWatch = new Stopwatch();
            while (secondWatch.elapsed() < 3 * Math.pow(10, 3)) {
              dashboard.goBackward(100);
            }
            dashboard.stop();
            if (!clawIsDown) {
              // Arms are currently up so drop them since the block is
              // detected and close by
              lifter.dropClaw();
              clawIsDown = true;
            }
            // Driving forward until the block is within 7 cm
            while (usLow.getFilteredDistance() >= 7) {
              dashboard.goForward(100);
            }
            juggle(); // Juggle to reposition the block so it is easier to pick
            dashboard.stop();
            // Drive forward 8 cm so the block will be picked up perfectly
            dashboard.goForward(8, false);
            // Remember where the block was picked up so the robot
            // will return here after dropping off in the designated zone
            previousPickUp.setLocation(odometer.getX(), odometer.getY());
            dashboard.stop();
            lifter.lift(); // Picking up the block
            isCarrying = true;
            // Driving back to the green zone (a bit further to account) for the arms
            // and at this distance, the block is dropped off in the middle of the zone
            navigation.travelTo(greenZone.getX() + 45, greenZone.getY() + 45, false);
            // If it is the nth*2 time, robot will stack.
            if (!firstIteration) {
              lifter.stackLift();
            }
            navigation.turnTo(180); // Turning so the block will end up in the middle of the zone
            dashboard.goForward(
                2,
                false); // Going forward 2 cm so even closer to block (used for stacking purposes)
            lifter.dropBlock();
            Stopwatch secondStop = new Stopwatch();
            // After dropping it off, back up then lift arms
            while (secondStop.elapsed() < 3.5 * Math.pow(10, 3)) {
              dashboard.goBackward(100);
            }
            dashboard.stop();
            lifter.postLift();
            firstIteration = !firstIteration; // Only making stacks of 2 so reverse boolean
            navigation.travelTo(
                greenZone.getX() - 15,
                greenZone.getY(),
                false); // Travel to side of green zone so blocks are not touched when navigating
                        // out of the zone
            navigation.travelTo(
                previousPickUp.getX(),
                previousPickUp.getY(),
                false); // Return to previous position of where styrofoam block was found
            doSearchAndDetect(
                greenZone.getX(), greenZone.getY()); // Do search and detect at the new position
          }
        }
      }
    }
  }