#include <Servo.h>

Servo servoLeft;
Servo servoRight;

int infraredState;
int front;                //button state for fron IR sensor
int iFront = 13;      //pin number for front IR sensor
int left;         //button state for left IR sensor
int iLeft = 4;       //pin number for left IR sensor
int right;         //button state for right IR sensor
int iRight = 5;      //pin number for right IR sensor
int trigPin = 7;
int echoPin = 8;
volatile unsigned int Rindex;
volatile unsigned int Lindex;
int xY [2];         //xY[0] stores array index and xY[1] stores binary string index
unsigned char dir = 1;        //determines direction
int arrIndex = 9;  //array index starts at 9 because its the right-most column of the maze
int binIndex = 0;  //bin index starts at 0 because its the bottom row of the maze



void setup() {
  // put your setup code here, to run once:
  servoLeft.attach(10);
  servoRight.attach(11);
  pinMode(iLeft, INPUT);
  pinMode(iRight, INPUT);
  pinMode(iFront, INPUT);
  attachInterrupt(0, rtEncoder, RISING);
  attachInterrupt(1, ltEncoder, RISING);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

}

void loop() {
  // put your main code here, to run repeatedly:

  servoRight.write(90);
  servoLeft.write(90);

  left = digitalRead(iLeft);
  right = digitalRead(iRight);
  front = digitalRead(iFront);

  while ( left == LOW && right == LOW && front == HIGH) {
    distanceTest();
    forward();
    delay(500);
    left = digitalRead(iLeft);
    right = digitalRead(iRight);
    front = digitalRead(iFront);

  }

  while ( right == HIGH) {
    distanceTest();
    turnRight();
    delay(200);
    forward();
    delay(500);
    right = digitalRead(iRight);
    front = digitalRead(iFront);
    left = digitalRead(iLeft);
  }
  while (left == LOW && right == LOW && front == LOW) {
    distanceTest();
    turnRight();
    turnRight();
    delay(500);
    left = digitalRead(iLeft);
    right = digitalRead(iRight);
    front = digitalRead(iFront);
  }

  while ( left == HIGH && right == LOW && front == LOW) {
    distanceTest();
    turnLeft();
    forward();
    delay(500);
    left = digitalRead(iLeft);
    right = digitalRead(iRight);
    front = digitalRead(iFront);
  }



}

void forward() {
 /*
  Rindex = 0;
  Lindex = 0;
  int testFront = HIGH;
  while (testFront == HIGH) {

    servoRight.write(180);
    servoLeft.write(0);
    testFront = digitalRead(iFront);
  }
  stopRobot();

 */
  Rindex = 0;
  Lindex = 0;
  while (Rindex < 32 && Lindex < 32) {
    servoRight.write(180);
    servoLeft.write(0);
    if ((Rindex >= 32) || (Lindex >= 32)) {
      stopRobot();
      break;
    }
  }

  if(dir == 1)
    binIndex += 1;
  if(dir == 2)
    arrIndex += 1;
  if(dir == 3)
    binIndex -= 1;
  if(dir == 4)
    arrIndex -= 1;

}

void forward(int index) {
  Rindex = 0;
  Lindex = 0;
  while (Rindex < index + 1 && Lindex < index + 1) {
    servoRight.write(180);
    servoLeft.write(0);


    if ((Rindex >= index + 1) || (Lindex >= index + 1)) {
      stopRobot();
      break;
    }
  }
}

void reverse() {
  servoLeft.write(180);
  servoRight.write(0);
}

void turnRight(int index) {
  Rindex = 0;
  Lindex = 0;

  while (Rindex < index + 1 && Lindex < index + 1) {
    servoRight.write(180);
    servoLeft.write(180);

    if ((Rindex >= index + 1) || (Lindex >= index + 1)) {
      stopRobot();
      break;
    }
  }
  // forward(32);


}

void turnRight() {
  Rindex = 0;
  Lindex = 0;

  if(dir == 1){
   dir += 3;
  }
  else
   dir -= 1;

  while (Rindex < 10 && Lindex < 10) {
    servoRight.write(180);
    servoLeft.write(180);

    if ((Rindex >= 10) || (Lindex >= 10)) {
      stopRobot();
      break;
    }
  }
  // forward(32);

}

void turnLeft() {
  Rindex = 0;
  Lindex = 0;

  if(dir == 4){
    dir -= 3;
  }
  else
    dir += 1;

  while (Rindex < 10 && Lindex < 10) {
    servoRight.write(0);
    servoLeft.write(0);
    if ((Rindex >= 10) || (Lindex >= 10)) {
      stopRobot();
      break;
    }
    //  forward(32);

  }
}

 void turnLeft(int spokes){
    Rindex = 0;
    Lindex = 0;
    while(Rindex < spokes && Lindex < spokes){
      servoRight.write(0);
      servoLeft.write(0);
      if((Rindex >= spokes) || (Lindex >= spokes)){
      stopRobot();
      break;
      }
    }
  }

void stopRobot() {
  servoLeft.write(90);
  servoRight.write(90);
}

void ltEncoder() {
  Lindex++;
}

void rtEncoder() {
  Rindex++;
}

double microsecondsToCentimeters(long microseconds){
  return (double) microseconds / 29 / 2;
}

void distanceTest(){
  double duration, cm;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  cm = microsecondsToCentimeters(duration);
  if( cm >= 4.9657 && cm <= 10){
    turnLeft(3);
  }
  if( cm <= 2.4257 && cm >= 0){
    turnRight(3);
  }
}