#include <Zumo32U4.h>

#define numberOfLineSensors 3

Zumo32U4Motors motors;
Zumo32U4LineSensors lineSensors;
Zumo32U4ProximitySensors proxSensors;
Zumo32U4Buzzer buzzer;

int motorSpeed = 100;
int lineSensorValues[numberOfLineSensors];
int lightThreshold = 1000;
int numberFound = 0;
int roomNumber = 0;

bool searching = false;
bool waiting = false;

void setup() {
  lineSensors.initThreeSensors();
  proxSensors.initThreeSensors();
  Serial1.begin(9600);
  delay(2000);
}

void loop() {
  do {
    autoMode();
  } while (waiting == false);
}

void autoMode() {
  lineSensors.read(lineSensorValues);
  motors.setSpeeds(motorSpeed, motorSpeed);
  if (lineSensorValues[0] > lightThreshold && lineSensorValues[numberOfLineSensors - 1] > lightThreshold)
  {
    delay(100);
    motors.setSpeeds(0, 0);
    waiting = true;
    Serial1.print("HIT WALL\n");
  }
  else if (lineSensorValues[0] > lightThreshold)
  {
    motors.setSpeeds(motorSpeed, -motorSpeed);
  }
  else if (lineSensorValues[2] > lightThreshold)
  {
    motors.setSpeeds(-motorSpeed, motorSpeed);
  }
  do {
    checkForInput();
  } while (waiting == true);
}

void checkForInput() {
  if (Serial1.available() > 0) {
    char inChar = (char)Serial1.read();
    switch (inChar) {
      case 'z': // Stop
        waiting = true;
        motors.setSpeeds(0, 0);
        Serial1.print("STOP\n");
        break;
      case 'x': // 180 Spin
        motors.setSpeeds(-100, 100);
        delay(1850);
        waiting = false;
        break;
      case 'w': // Forward
        waiting = false;
        motors.setSpeeds(100, 100);
        break;
      case 's': // Back
        waiting = false;
        motors.setSpeeds(-100, -100);
        break;
      case 'd': // Turn Right
        motors.setSpeeds(-100, -100);
        delay(250);
        turnRight();
        break;
      case 'a': // Turn Left
        motors.setSpeeds(-100, -100);
        delay(250);
        turnLeft();
        break;
      case 'q': // Search Left
        Serial1.print("SEARCHING\n");
        turnLeft();
        searchingRoom();
        break;
      case 'e': // Search Right
        Serial1.print("SEARCHING\n");
        turnRight();
        searchingRoom();
        break;
    } // end switch
  } // end if
}

void turnLeft() {
  motors.setSpeeds(100, -100);
  delay(850);
  waiting = false;
}

void turnRight() {
  motors.setSpeeds(-100, 100);
  delay(850);
  waiting = false;
}

void searchingRoom() {
  searching = true;
  int i = 0;
  while (searching == true) {
    turnLeft();
    i++;
    if (proxSensors.countsFrontWithLeftLeds() > 5 ) {
      buzzer.playNote(NOTE_A(4), 250, 8);
      roomNumber += 1;
      numberFound += 1;
      Serial1.print(roomNumber);
      while (i < 3) {
        i++;
        turnLeft();
      }
      searching = false;
    }
    else if (i > 3) {
      searching = false;
    }
  }
  waiting = false;
}