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] = {'R', 'L', 'F', 'L', 'R'};   // 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 back()
{
  Serial.println("BACKWARD");
  // Two motors on one side
  digitalWrite(out1, HIGH);
  digitalWrite(out2, LOW);

  digitalWrite(out3, LOW);
  digitalWrite(out4, HIGH);
}
 
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(150);
  left();
  delay(1300);
  while(digitalRead(EX_RIGHT) == 0) left();
  readSensors();
}

void right_90() {
  forward();
  delay(150);
  right();
  delay(1300);
  while(digitalRead(EX_LEFT) == 0) right();
//  Stop();
  readSensors();
//  delay(60000);
}

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() {
  char c;
  if(Serial.available() > 0){
    c = Serial.read();
    if(c == 'w')
      control = true;
    else if(c == 'W')
      control = false;
//    Stop(); //initialize with motors stoped
    //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(){
  //printing values of the sensors to the serial monitor
//  readSensors();

  //line detected by both
  if(digitalRead(LEFT)==0 && digitalRead(RIGHT)==0){
//    Serial.println("Forward");
    //Forward
    forward();
  }
  //line detected by left sensor
  else if(digitalRead(LEFT)==0 && !analogRead(RIGHT)==0){
    //turn left
//    Serial.println("Left");
    right();
  }
  //line detected by right sensor
  else if(!digitalRead(LEFT)==0 && digitalRead(RIGHT)==0){
    //turn right
//    Serial.println("Right");
    left();
  }
  // Intersection detected
  else if(!digitalRead(LEFT)==0 && !digitalRead(RIGHT)==0 && !digitalRead(EX_LEFT)==0 && !digitalRead(EX_RIGHT)==0) {
//    Serial.println("Intersection");
    char dir;
    if(Serial.available() > 0) {
      Stop();
      dir = Serial.read();
      Serial.println(dir);
      if(dir == 'L')
      left_90();
      else if(dir == 'R')
        right_90();
      else if(dir == 'F') {
        forward();
        delay(200);
      }
    }
    Serial.println(dir);
//    index_dir++;
//    if(index_dir == 5)
//      index_dir = 0;
  }
  //line detected by none
  else if(!digitalRead(LEFT)==0 && !digitalRead(RIGHT)==0){
    //stop1
//    Serial.println("Stop");
    Stop();
  }
}
