int out1 = 2;
int out2 = 3;

int out3 = 4;
int out4 = 5;

// defining pins and variables
#define LEFT A0
#define RIGHT A1
#define EX_LEFT A2
#define EX_RIGHT A3

char next_dir[5] = {'F', 'L', 'L', 'R', 'F'};   // Next direction
int index_dir = 0;
 
void setup()
{       
  Serial.begin(9600);  //Set the baud rate to your Bluetooth module.
  // Motors pins
  pinMode(out1, OUTPUT);
  pinMode(out2, OUTPUT);
  pinMode(out3, OUTPUT);
  pinMode(out4, OUTPUT);
  //declaring pin types
  pinMode(LEFT, INPUT);
  pinMode(RIGHT, INPUT);
  pinMode(EX_LEFT, INPUT);
  pinMode(EX_RIGHT, INPUT);
}
 
void forward()
{
//  Serial.println("FORWARD");
  // Two motors on one side
  digitalWrite(out1, LOW);
  digitalWrite(out2, HIGH);

  digitalWrite(out3, HIGH);
  digitalWrite(out4, LOW);
}
 
void left()
{
//  Serial.println("LEFT");
  digitalWrite(out1, LOW);
  digitalWrite(out2, HIGH);

  digitalWrite(out3, LOW);
  digitalWrite(out4, HIGH);
}
 
void right()
{
//  Serial.println("RIGHT");
  digitalWrite(out1, HIGH);
  digitalWrite(out2, LOW);

  digitalWrite(out3, HIGH);
  digitalWrite(out4, LOW);
}
 
void Stop()
{
  digitalWrite(out1, LOW);
  digitalWrite(out2, LOW);

  digitalWrite(out3, LOW);
  digitalWrite(out4, LOW);
}

void left_90() {
  forward();
  delay(100);
  left();
  delay(2000);
  while(digitalRead(EX_RIGHT) == 0) left();
  readSensors();
}

void right_90() {
  forward();
  delay(100);
  right();
  delay(2000);
  while(digitalRead(EX_LEFT) == 0) right();
  readSensors();
}

void readSensors() {
  Serial.print("left = ");
  Serial.println(digitalRead(LEFT));

  Serial.print("right = ");
  Serial.println(digitalRead(RIGHT));

  Serial.print("Exleft = ");
  Serial.println(digitalRead(EX_LEFT));

  Serial.print("Exright = ");
  Serial.println(digitalRead(EX_RIGHT));
}


void loop(){
  if(index_dir == 5) {
    Stop();
  }
  //line detected by both
  else if(digitalRead(LEFT)==0 && digitalRead(RIGHT)==0){
    //Forward
    forward();
  }
  //line detected by left sensor
  else if(digitalRead(LEFT)==0 && !analogRead(RIGHT)==0){
    //turn left
    right();
  }
  //line detected by right sensor
  else if(!digitalRead(LEFT)==0 && digitalRead(RIGHT)==0){
    //turn right
    left();
  }
  // Intersection detected
  else if(!digitalRead(LEFT)==0 && !digitalRead(RIGHT)==0 && !digitalRead(EX_LEFT)==0 && !digitalRead(EX_RIGHT)==0) {
    if(next_dir[index_dir] == 'L')
      left_90();
    else if(next_dir[index_dir] == 'R')
      right_90();
    else if(next_dir[index_dir] == 'F') {
      forward();
      delay(200);
    }
    Serial.println(index_dir);
    index_dir++;
  }
  else {
    forward();
  }
}