#include <SoftwareSerial.h> 
#include "directions.h"
#include "servo.h"
#include "US_IR.h"
#include "Auto_Manual.h"
dir D;
ser S;
sens UI;
a_m AM;
SoftwareSerial Blue(2,3); 
int distance_F,distance_L,distance_R;
bool autoMode = true;

void setup() {
  Blue.begin(9600); 
  D.s_dir();
  S.s_ser();
  UI.s_sen();
  AM.s_am();
}

void loop() {
  if (Blue.available()) {
    char cmd = Blue.read();
  switch (cmd) {
      case 'A': 
        AM.Auto();
        Blue.println("Auto Mode ON");
        break;
      
      case 'M': 
        AM.Manual();
        Blue.println("Manual Mode ON");
        break;
      case 'F': D.forword(); break;
      case 'B': D.backword(); break;
      case 'L': D.turnLeft(); break;
      case 'R': D.turnRight(); break;
      case 'S': D.Stop(); break;
  }  }

  if (autoMode) {
    distance_F = UI.Ultrasonic_read();
    if ((digitalRead(R_S) == 0) && (digitalRead(L_S) == 0)) {
      if (distance_F > 15) {
        D.forword();
      } else {
        Check_side();
      }
    } else if ((digitalRead(R_S) == 1) && (digitalRead(L_S) == 0)) {
      D.turnRight();
    } else if ((digitalRead(R_S) == 0) && (digitalRead(L_S) == 1)) {
      D.turnLeft();
    }
  }
}


void compareDistance() 
{ 
  if (distance_L > distance_R) 
  {
    D.turnLeft();
    delay(500);
    D.forword();
    delay(600);
    D.turnRight();
    delay(500);
    D.forword();
    delay(600);
    D.turnRight();
    delay(400);
  }
  
  else 
  {
    D.turnRight();
    delay(500);
    D.forword();
    delay(600);
    D.turnLeft();
    delay(500);
    D.forword();
    delay(600);
    D.turnLeft();
    delay(400);
  }
 }

void Check_side() {
  D.Stop();
  delay(100);
  
  for (int angle = 70; angle <= 140; angle += 5) {
    S.servoPulse(servo, angle);
  }
  delay(300);
  distance_R = UI.Ultrasonic_read();
  Serial.print("D R="); Serial.println(distance_R);
  delay(100);

  for (int angle = 140; angle >= 0; angle -= 5) {
    S.servoPulse(servo, angle);
  }
  delay(500);
  distance_L = UI.Ultrasonic_read();
  Serial.print("D L="); Serial.println(distance_L);
  delay(100);

  for (int angle = 0; angle <= 70; angle += 5) {
    S.servoPulse(servo, angle);
  }
  delay(300);
  compareDistance();
}