// Method scans input and maps it; if obstacle is in front, left and right,
  // backpedal.  If there is a space adjacent but an obstacle at the front,
  // turns to move around the obstacle using the available space.
  public static void movRow() {
    double nextCell = mapObj.basicProb(); // work out probability
    // output to RConsole through BlueTooth
    // btObj.stringToRCon("Object Probability in next Cell: %2f" + nextCell);

    // mapObj.printMap(columns, numOfRowCells);

    // sonar scan around of the robot
    boolean[] scanResults = new boolean[3];
    scanResults = mapObj.scanAll();
    mapObj.updateMap(scanResults);
    // btObj.stringToRCon("Scan Results: " + scanResults[0] + ", " + scanResults[1] + ", " +
    // scanResults[2]);
    // mapObj.printMap(columns, numOfRowCells);

    if (scanResults[1]) { // if there's an obstacle in front
      // if there's an obstacle to the left & right.  i.e. a dead end
      if (scanResults[0] && scanResults[2]) {
        turnAround(scanResults[2], mapObj, movObj); // move backwards
      } else {
        // call movAround() method to navigate around the obstacle
        movAround(scanResults[1], mapObj, movObj);
      }
    } else {
      movObj.nextCell(mapObj); // move forward a cell
    }
  }
  // Method to move around an obstacle - runs when obstacle is in front
  public static void movAround(boolean r, MapSystem ms, Movement mv) {
    mv.turn(r, ms); // true = right turn; false = left turn
    movRow(); // scan and move forward if necessary
    mv.turn(!r, ms); // invert the boolean to turn the other way

    // move forward twice to pass the obstacle
    movRow();
    ++loop2;
    movRow();
    ++loop2;

    mv.turn(!r, ms); // turn to face original column
    boolean extend = mapObj.scanAhead(); // flag to check object is passed
    while (extend) {
      mv.turn(r, ms); // turn to move along the axis
      movObj.nextCell(mapObj);
      ++loop2;
      mv.turn(!r, ms); // face the original column
      if (!mapObj.scanAhead()) { // if an obstacle is not found anymore
        extend = false; // break the while loop
      } else {
      }
    }
    movRow(); // move into the original column
    mv.turn(r, ms); // face the correct way to continue the patrol
  }
  // method runs when robot is in a dead end and has to back up and go around
  public static void turnAround(boolean r, MapSystem ms, Movement mv) {
    mv.turn(ms); // 180 degree turn
    movRow(); // move forward

    // check for empty adjacent space
    boolean[] scanResults = new boolean[3];
    scanResults = mapObj.scanAll();
    // Move to said empty space
    if (scanResults[0] && !scanResults[2]) { // Obstacle on the Left
      mv.turn(scanResults[0], ms); // turn right
      movRow();
    } else { // Obstacle on the Right
      mv.turn(!scanResults[2], ms); // turn left
      movRow();
      mv.turn(!scanResults[2], ms);
      movRow();
      // move forward 2 cells, scanning as we go
      // if an obstacle is still on the same axis as the first
      //    class it as a different obstacle
      //    move forward a cell
      //    scan again - recursive
      // move back to the correct column

      // if at a 'limit cell', turn left instead of right

    }
  }
  public static void main(String[] args) {
    // grid values for mapping
    columns = 4;
    numOfRowCells = 5;
    cellSize = 30;

    // connect to other classes
    mapObj = new MapSystem(cellSize, columns, numOfRowCells);
    movObj = new Movement(cellSize, columns, numOfRowCells);

    // connect to RConsole
    btObj.startBtConn();
    System.out.println("Press Any Button");
    Button.waitForAnyPress();
    LCD.clear();

    // for each column
    for (loop1 = 0; loop1 < columns; loop1++) {
      // for each cell within the column
      for (loop2 = 0; loop2 < numOfRowCells; loop2++) {
        movRow(); // scan and move method
      }
      if (loop1 == (columns - 1)) {
        break;
      }
      if (loop1 % 2 == 0) { // if the robot moves along the Y axis positively
        movCol(true); // turn right into the new column
      } else {
        movCol(false); // turn left  into the new column
      }
    }

    String[] output = mapObj.getMap(columns, numOfRowCells);
    Button.waitForAnyPress();
    btObj.stringToRCon(output[0]);
    btObj.stringToRCon(output[1]);
    btObj.stringToRCon(output[2]);
  }