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] = {'L', 'R', 'R', 'F', 'L'};   // Next direction
int index_dir = 0;
 
char command;
bool control = true;
 
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 loop() {
  char c;
  if(Serial.available() > 0){
    c = Serial.read();
    if(c == 'w')
      control = true;
    else if(c == 'W')
      control = false;
//    Stop(); //initialize with motors stopped
    //Change pin mode only if new command is different from previous.   
    //Serial.println(command);
  }
  if(control)
    manual();
  else if(!control)
    automatic();
}

void manual() {
//  Serial.println("In manual");
  if(Serial.available() > 0) {
    Stop();
    command = Serial.read();
    Serial.println(command);
    switch(command){
    case 'F':  
      forward();
      break;
    case 'B':  
       back();
      break;
    case 'L':  
      left();
      break;
    case 'R':
      right();
      break;
    }
  }
}

void automatic() {
  if(index_dir == 1) {
    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) {
    Serial.println("Intersection here");
    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();
  }
}
 
void forward()
{
//  Serial.println("FORWARD");
  // Two motors on one side
  digitalWrite(out1, LOW);
  digitalWrite(out2, HIGH);

  digitalWrite(out3, HIGH);
  digitalWrite(out4, LOW);
}
 
void back()
{
  Serial.println("BACKWARD");
  // Two motors on one side
  digitalWrite(motor1pin1, HIGH);
  digitalWrite(motor1pin2, LOW);

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

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

void left_90() {
  forward();
  delay(120);
  left();
  delay(1000);
  while(digitalRead(EX_RIGHT) == 0) left();
  while(digitalRead(RIGHT) == 0) left();
  readSensors();
}

void right_90() {
  forward();
  delay(120);
  right();
  delay(1000);
  while(digitalRead(EX_LEFT) == 0) right();
  while(digitalRead(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 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);
}
