#include <Servo.h>

// Declare variables for line sensors
int s1;
int s2;
int s3;
int s4;
int s5;

// Ultrasonic sensor pins
const int trigPinFront = 8;
const int echoPinFront = 7;
const int trigPinLeft = 10;
const int echoPinLeft = 11;
const int trigPinRight = 12;
const int echoPinRight = 13;

const int ENA = 13;
const int IN1 = A5;
const int IN2 = A4;
const int ENB = 12;
const int IN3 = A3;
const int IN4 = A2;

int Speed = 90;    // Speed of this robot for forward motion
int SpeedRL = 100; // Speed of this robot for left and right turns

Servo lineFollowingServo;  // Servo motor for the line-following part
bool hasTurned180 = false; // Flag to track if the robot has turned 180 degrees

int dis = 5;  // Define the distance threshold

// Function declarations
void carforward();
void carturnleft();
void carturnright();
void carStop();
void carturn180();  // New function to turn the robot 180 degrees
long getUltrasonicDistance(int trigPin, int echoPin); // Function to get distance from ultrasonic sensor

void setup() {
  Serial.begin(9600); // Initialize Serial Monitor

  // Set up pins for the line sensors
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);

  // Set up motor control pins
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  
  // Initialize servos
  lineFollowingServo.attach(9);  // Attach the line-following servo to pin 9
  lineFollowingServo.write(0);   // Initial position of the line-following servo is set to 0 degrees

  // Initialize ultrasonic sensor pins
  pinMode(trigPinFront, OUTPUT);
  pinMode(echoPinFront, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
}

void loop() 
{
  // Read the values from the line sensors
  s1 = digitalRead(2);
  s2 = digitalRead(3);
  s3 = digitalRead(4);
  s4 = digitalRead(5);
  s5 = digitalRead(6);

  // white = 0, black = 1
  
  if ((s1 == 0 && s2 == 0 && s3 == 1 && s4 == 0 && s5 == 0) ||
      (s1 == 0 && s2 == 1 && s3 == 1 && s4 == 1 && s5 == 0)) {
    carforward();
    Serial.println("Moving Forward");
  } 
  else if (s1 == 1 && s2 == 1 && s3 == 1 && s4 == 1 && s5 == 1) {
    carStop();  // Stop the car
    Serial.println("Stopping");

    if (!hasTurned180) {  // If the robot hasn't turned 180 degrees yet
      lineFollowingServo.write(100);  // Rotate the line-following servo motor to 90 degrees
      Serial.println("Servo at 90 degrees");
      delay(2000);  // Wait for the servo to complete its motion
      carturn180(); // Turn the robot 180 degrees
      Serial.println("Turning 180 degrees");
      hasTurned180 = true; // Set the flag to indicate the robot has turned 180 degrees
    } else {
      lineFollowingServo.write(0);  // Reset the line-following servo motor to its original position
      Serial.println("Servo at 0 degrees");
    }
  } 
  else if ((s1 == 1 && s2 == 1 && s3 == 1 && s4 == 0 && s5 == 0) ||
           (s1 == 0 && s2 == 1 && s3 == 1 && s4 == 0 && s5 == 0) ||
           (s1 == 0 && s2 == 1 && s3 == 0 && s4 == 0 && s5 == 0) ||
           (s1 == 1 && s2 == 1 && s3 == 0 && s4 == 0 && s5 == 0) ||
           (s1 == 1 && s2 == 0 && s3 == 0 && s4 == 0 && s5 == 0)) {
    carturnleft();
    Serial.println("Turning Left");
  } 
  else if ((s1 == 0 && s2 == 0 && s3 == 1 && s4 == 1 && s5 == 1) ||
           (s1 == 0 && s2 == 0 && s3 == 0 && s4 == 1 && s5 == 1) ||
           (s1 == 0 && s2 == 0 && s3 == 0 && s4 == 0 && s5 == 1)) {
    carforward();
    delay(300);
    Serial.println("Moving Forward");
            
    if ((s1 == 0 && s2 == 0 && s3 == 1 && s4 == 0 && s5 == 0) ||
        (s1 == 0 && s2 == 1 && s3 == 1 && s4 == 1 && s5 == 0)) {
        carforward(); // Continue moving forward
        Serial.println("Continuing Forward");
    } else {
        carturnright();
        Serial.println("Turning Right");
    }
  } 
  // Call the maze-solving function when line-following is not required
  else if (s1 == 0 && s2 == 0 && s3 == 0 && s4 == 0 && s5 == 0) {
    // Maze-solving logic with 3 ultrasonic sensors
    long frontDistance = getUltrasonicDistance(trigPinFront, echoPinFront);
    long leftDistance = getUltrasonicDistance(trigPinLeft, echoPinLeft);
    long rightDistance = getUltrasonicDistance(trigPinRight, echoPinRight);
    
    if (frontDistance > dis && leftDistance < dis && rightDistance < dis) {
        carforward();
        Serial.println("Moving Forward");
    } else if (leftDistance > dis && frontDistance < dis && rightDistance < dis) {
        carturnleft();
        Serial.println("Turning Left");
    } else if (rightDistance > dis && frontDistance < dis && leftDistance < dis) {
        carturnright();
        Serial.println("Turning Right");
    } else {
        carStop();  // Stop the car if no specific condition is met
        Serial.println("Stopping");
    }
  }
}

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

void carturnleft() {
  analogWrite(ENA, 0);
  analogWrite(ENB, SpeedRL);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void carturnright() {
  analogWrite(ENA, SpeedRL);
  analogWrite(ENB, 0);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

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

void carturn180() {
  // Turn the robot 180 degrees
  analogWrite(ENA, SpeedRL);
  analogWrite(ENB, 0);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  delay(1000);  // Adjust this delay to achieve a 180-degree turn
  carStop();    // Stop the car after the turn
}

long getUltrasonicDistance(int trigPin, int echoPin) {
  // Function to measure distance using an ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) / 29.1;  // Convert duration to distance in cm
  
  return distance;
}