#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>  // Include the Servo library

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Pins for ultrasonic sensors
const int frontTrigPin = 3;
const int frontEchoPin = 2;
const int backTrigPin = 4;
const int backEchoPin = 5;
const int leftTrigPin = 6;
const int leftEchoPin = 7;
const int rightTrigPin = 8;
const int rightEchoPin = 9;

// Servo motor setup
Servo motorForwardBackward;  // Servo to control forward and backward movement
Servo motorLeftRight;        // Servo to control direction (left and right)

// LED pins
const int leftLedPin = 12;   // Left LED
const int rightLedPin = 13;  // Right LED
const int centerLedPin = A0; // Center LED (for front and back obstacle)

// Function to read distance from ultrasonic sensor
int readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.0340 / 2;
  return distance;
}

void setup() {
  lcd.begin(16, 2); // Initialize the LCD
  lcd.backlight();

  // Set up ultrasonic sensor pins
  pinMode(frontTrigPin, OUTPUT);
  pinMode(frontEchoPin, INPUT);
  pinMode(backTrigPin, OUTPUT);
  pinMode(backEchoPin, INPUT);
  pinMode(leftTrigPin, OUTPUT);
  pinMode(leftEchoPin, INPUT);
  pinMode(rightTrigPin, OUTPUT);
  pinMode(rightEchoPin, INPUT);

  // Attach servos to corresponding pins
  motorForwardBackward.attach(10);  // Attach forward/backward servo to pin 10
  motorLeftRight.attach(11);        // Attach left/right servo to pin 11

  // Set up LED pins
  pinMode(leftLedPin, OUTPUT);
  pinMode(rightLedPin, OUTPUT);
  pinMode(centerLedPin, OUTPUT);

  Serial.begin(9600);
}

void loop() {
  int frontDistance = readDistance(frontTrigPin, frontEchoPin);
  int backDistance = readDistance(backTrigPin, backEchoPin);
  int leftDistance = readDistance(leftTrigPin, leftEchoPin);
  int rightDistance = readDistance(rightTrigPin, rightEchoPin);

  // Display distance on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("F:");
  lcd.print(frontDistance);
  lcd.print(" B:");
  lcd.print(backDistance);
  lcd.setCursor(0, 1);
  lcd.print("L:");
  lcd.print(leftDistance);
  lcd.print(" R:");
  lcd.print(rightDistance);

  // Control servos based on distance from obstacles and blink LEDs accordingly
  if (frontDistance < 20 && backDistance < 20) {
    // Obstacle in both front and back, stop and blink center LED
    motorForwardBackward.write(90);  // Stop the car
    blinkLed(centerLedPin, 500);     // Blink center LED
    lcd.clear();
    lcd.print("Obstacle F/B");
    delay(2000);
  } else if (frontDistance < 20) {
    // Obstacle in front, stop and reverse
    motorForwardBackward.write(180);  // Move backward (adjust servo angle as needed)
    blinkLed(centerLedPin, 500);      // Blink center LED
    lcd.clear();
    lcd.print("Obstacle in Front");
    delay(2000);
  } else if (backDistance < 20) {
    // Obstacle at the back, stop and move forward
    motorForwardBackward.write(0);    // Move forward (adjust servo angle as needed)
    blinkLed(centerLedPin, 500);      // Blink center LED
    lcd.clear();
    lcd.print("Obstacle at Back");
    delay(2000);
  } else {
    // No obstacle, move forward
    motorForwardBackward.write(90);   // Neutral position (stop or normal forward movement)
    digitalWrite(centerLedPin, LOW);  // Turn off center LED
  }

  // Turn based on left and right sensor readings and blink corresponding LED
  if (leftDistance < 20) {
    // Obstacle on the left, turn right and blink right LED
    motorLeftRight.write(45);  // Adjust right (servo angle for turning right)
    blinkLed(rightLedPin, 300); // Blink right LED
    lcd.clear();
    lcd.print("Turning Right");
    delay(1000);
  } else if (rightDistance < 20) {
    // Obstacle on the right, turn left and blink left LED
    motorLeftRight.write(135);  // Adjust left (servo angle for turning left)
    blinkLed(leftLedPin, 300);  // Blink left LED
    lcd.clear();
    lcd.print("Turning Left");
    delay(1000);
  } else {
    // No obstacle, keep direction straight
    motorLeftRight.write(90);  // Neutral position (servo angle for straight direction)
    digitalWrite(leftLedPin, LOW);   // Turn off left LED
    digitalWrite(rightLedPin, LOW);  // Turn off right LED
  }

  delay(100);  // Small delay to stabilize readings
}

// Function to blink an LED at a specific interval
void blinkLed(int ledPin, int interval) {
  digitalWrite(ledPin, HIGH);
  delay(interval);
  digitalWrite(ledPin, LOW);
  delay(interval);
}