#include <Servo.h>

const unsigned int numInputs = 5;
unsigned char pins[] = {
  7,6,5,4,3};
unsigned char startButton = 2;
unsigned char LED = 11;
unsigned int avals [numInputs];

unsigned int* farLeft = &avals[0];
unsigned int* left = &avals[1];
unsigned int* middle = &avals[2];
unsigned int* right = &avals[3];
unsigned int* farRight = &avals[4];

Servo servoRight;
Servo servoLeft;

int roboSpeed = 10;

enum Mode {
  m_normal = 0, m_turn_left, m_turn_right, m_turn_left_crossing, m_turn_right_crossing };
enum ModeNextCrossing {
  m_take_left_turn = 0, m_take_right_turn, m_ignore_next };

Mode mode = m_normal;
ModeNextCrossing modeNextCrossing = m_take_left_turn;

boolean started = false;

void setup(void) {
  Serial.begin(115200);

  // Initialize pins
  for (int i = 0; i < numInputs; i++) {
    pinMode(pins[i], INPUT);
  }
  pinMode(startButton, INPUT_PULLUP);
  servoLeft.attach(12);
  servoRight.attach(13);
}

void loop() {
  // Once startButton was pressed, switch to "started" mode
  //if (!digitalRead(startButton)) {
  //  started = true;
  //}

  if (!digitalRead(startButton)) {
    started = !started;
    if (!started) {
      mode = m_normal;
    }
    delay(500);
  }

  if (!started) {
    // If not started, stop servos
    servoRight.writeMicroseconds(1500);
    servoLeft.writeMicroseconds(1500);
  }
  else {
    // Read inputs from infrared sensors (and print them and the button state)
    for (int i = 0; i < numInputs; i++) {
      avals[i] = !digitalRead(pins[i]);
      //Serial.print(avals[i]);
    }
    //Serial.print(digitalRead(startButton));
    //Serial.println("=============");

    // Strategy: If middle IR sees black, drive forward. If left, but not far
    // left sees black, steer left (drive around left wheel, same for right wheel).
    // If only far left/right IR sensor sees black, steer right/left while
    // driving backwards. In other cases drive forward.
    // Overall speed is set to 10% to not overshoot the black lines

    if (mode == m_normal) {
      driveNormalMode();
    }
    else if (mode == m_turn_left) {
      driveTurnLeft();
    }
    else if (mode == m_turn_right) {
      driveTurnRight();
    }
    else if (mode == m_turn_left_crossing) {
      driveTurnLeftCrossing();
    }
    else if (mode == m_turn_right_crossing) {
      driveTurnRightCrossing();
    }

  }
}

void drive(int percLeft, int percRight) {
  servoRight.writeMicroseconds(1500 - percRight * 2);
  servoLeft.writeMicroseconds(1500 + percLeft * 2);
}

void driveNormalMode() {
  if (modeNextCrossing != m_ignore_next && *middle && (*left || *right)) {
    if (modeNextCrossing == m_take_left_turn) {
      mode = m_turn_left_crossing;
    }
    else {
      mode = m_turn_right_crossing;
    }
  }
  else if (*middle) {
    drive(roboSpeed, roboSpeed);
  }
  /*else if (*left && !*middle) {
    drive(0, roboSpeed);
  }
  else if (*right && !*farRight) {
    drive(roboSpeed, 0);
  }*/
  else if (*left) {
    //drive(-roboSpeed, 0);
    mode = m_turn_left;
  }
  else if (*right) {
    //drive(0, -roboSpeed);
    mode = m_turn_right;
  }
  else {
    drive(roboSpeed, roboSpeed);
  }

  if (*farLeft && *farRight && !*left && !*right && *middle) { // TODO: Check this
    started = false;
    mode = m_normal;
  }
  else if (*farRight) {
    modeNextCrossing = m_take_right_turn;
  }
  else if (*farLeft) {
    modeNextCrossing = m_take_left_turn;
  }
}

void driveTurnLeftCrossing() {
  Serial.println("driveTurnLeftCrossing");
  drive(-roboSpeed*0, roboSpeed);
  if (*middle + *left + *right == 0) {
    mode = m_normal;
    modeNextCrossing = m_ignore_next;
  }
}

void driveTurnRightCrossing() {
  drive(roboSpeed, -roboSpeed*0);
  if (*middle + *left + *right == 0) { // *middle && !*left && !*right
    mode = m_normal;
    modeNextCrossing = m_ignore_next;
  }
}

// Other helper functions

// void driveForward(int perc) {
//   servoRight.writeMicroseconds(1500 - perc * 2);
//   servoLeft.writeMicroseconds(1500 + perc * 2);
// }
// void turnLeft(int perc) {
//   servoRight.writeMicroseconds(1500 - perc * 2);
//   servoLeft.writeMicroseconds(1500 - perc * 2);
// }
// void turnRight(int perc) {
//   servoRight.writeMicroseconds(1500 + perc * 2);
//   servoLeft.writeMicroseconds(1500 + perc * 2);
// }
// void stopDriving() {
//   servoRight.writeMicroseconds(1500);
//   servoLeft.writeMicroseconds(1500);
// }

// boolean allWhite() {
//   for (int i = 0; i < 5; i++) {
//     if (avals[i] == 1)
//       return false;
//   }
//   return true;
// }

void driveTurnLeft() {
  drive(-roboSpeed, roboSpeed);
  if (*middle) {
    mode = m_normal;
  }
}

void driveTurnRight() {
  drive(roboSpeed, -roboSpeed);
  if (*middle) {
    mode = m_normal;
  }
}