#include <LiquidCrystal_I2C.h>

//**********Dc Motor L298N Driver Connection**********//
#define ENA 7  
#define IN1 8  
#define IN2 9  
#define IN3 10  
#define IN4 11  
#define ENB 12 
//*************************************************//

//**********5 Channel IR Sensor Connection**********//
#define S1 22
#define S2 23
#define S3 24
#define S4 25
#define S5 26
//*************************************************//

//**********Ultrasonic Sensor Connection**********//
#define FRONT_TRIG 27
#define FRONT_ECHO 28
#define BACK_TRIG 29
#define BACK_ECHO 30
#define FILES_TRIG 31
#define FILES_ECHO 32
//*************************************************//

#define LINE_DETECTED 0
#define DISTANCE_VALUE 8 //means 8cm

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int MOTOR_SPEED = 200;

void setup() {
    Serial.begin(9600);
    lcd.init(); // initialize the lcd
    lcd.backlight();
    //Motor Setup
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);
    pinMode(ENA, OUTPUT);
    pinMode(ENB, OUTPUT);

    //Infrared Setup
    pinMode(S1, INPUT);
    pinMode(S2, INPUT);
    pinMode(S3, INPUT);
    pinMode(S4, INPUT);
    pinMode(S5, INPUT);

    //Ultrasonic Setup
    pinMode(FRONT_TRIG, OUTPUT);
    pinMode(FRONT_ECHO, INPUT);
    pinMode(BACK_TRIG, OUTPUT);
    pinMode(BACK_ECHO, INPUT);
    pinMode(FILES_TRIG, OUTPUT);
    pinMode(FILES_ECHO, INPUT);

    stopMotor();
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Welcome");
    delay(1000);
}//END

void stopMotor(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void forwardMotor(){
  analogWrite(ENA, MOTOR_SPEED); 
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void turnRightMotor(){
  analogWrite(ENA, MOTOR_SPEED); 
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void turnLeftMotor(){
  analogWrite(ENA, MOTOR_SPEED); 
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

int frontDistance_cm(){ 
  // Clears the trigPin
  digitalWrite(FRONT_TRIG, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(FRONT_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(FRONT_TRIG, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(FRONT_ECHO, HIGH);
  // Calculating the distance
  int distance = duration * 0.034 / 2;
  int distance_cm = round(distance);
  return distance_cm;
}//end

int backDistance_cm(){
  // Clears the trigPin
  digitalWrite(BACK_TRIG, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(BACK_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(BACK_TRIG, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(BACK_ECHO, HIGH);
  // Calculating the distance
  int distance = duration * 0.034 / 2;
  int distance_cm = round(distance);
  return distance_cm;
}//end

int filesDistance_cm(){
  // Clears the trigPin
  digitalWrite(FILES_TRIG, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(FILES_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(FILES_TRIG, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  long duration = pulseIn(FILES_ECHO, HIGH);
  // Calculating the distance
  int distance = duration * 0.034 / 2;
  int distance_cm = round(distance);
  return distance_cm;
}//end

void loop() {
    //Reading Sensor Values
    int s1 = digitalRead(S1);  //Left Most Sensor
    int s2 = digitalRead(S2);  //Left Sensor
    int s3 = digitalRead(S3);  //Middle Sensor
    int s4 = digitalRead(S4);  //Right Sensor
    int s5 = digitalRead(S5);  //Right Most Sensor

    int frontDistance = frontDistance_cm();
    int backDistance = backDistance_cm();
    int fileDistance = filesDistance_cm();

    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(frontDistance);
    lcd.setCursor(10,0);
    lcd.print(backDistance);
    lcd.setCursor(0,1);
    lcd.print(fileDistance);
    delay(500);
    if (frontDistance < 10 || backDistance < 10){
        stopMotor();
    } else {
        //if only middle sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going forward with full speed 
            forwardMotor();
        }
        
        //if only left sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == LINE_DETECTED) && (s3 == !LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going left with full speed 
            turnLeftMotor();
        }
        
        //if only left most sensor detects black line
        if((s1 == LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == !LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going left with full speed 
            turnLeftMotor();
        }

        //if middle and left sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going left with full speed 
            turnLeftMotor();
        }

        //if middle, left and left most sensor detects black line
        if((s1 == LINE_DETECTED) && (s2 == LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going left with full speed 
            turnLeftMotor();
        }

        //if only right sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == !LINE_DETECTED) && (s4 == LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going right with full speed 
            turnRightMotor();
        }

        //if only right most sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == !LINE_DETECTED) && (s4 == !LINE_DETECTED) && (s5 == LINE_DETECTED))
        {
            //going right with full speed 
        turnRightMotor();
        }

        //if middle and right sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == LINE_DETECTED) && (s5 == !LINE_DETECTED))
        {
            //going right with full speed 
            turnRightMotor();
        }

        //if middle, right and right most sensor detects black line
        if((s1 == !LINE_DETECTED) && (s2 == !LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == LINE_DETECTED) && (s5 == LINE_DETECTED))
        {
            //going right with full speed 
            turnRightMotor();
        }

        //if all sensors are on a black line
        if((s1 == LINE_DETECTED) && (s2 == LINE_DETECTED) && (s3 == LINE_DETECTED) && (s4 == LINE_DETECTED) && (s5 == LINE_DETECTED))
        {
            //stop
            stopMotor();
        }
    }
}//endv