#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Pin setup for LEDs (Turn indicators)
int leftLED = 2;   // Left turn LED
int rightLED = 4;  // Right turn LED
int frontLED = 3;  // Front indicator LED

// Joystick pins
int joyX = A0;     // Joystick X-axis
int joyY = A1;     // Joystick Y-axis

// Servo Motors for steering
Servo leftServo;   // Steering left
Servo rightServo;  // Steering right

int leftServoPin = 5;
int rightServoPin = 6;

// Neutral positions for servos
int neutralTurnAngle = 90;    // Neutral (center) position for left and right servos

// Ultrasonic Sensor Pins
#define FRONT_TRIG 7
#define FRONT_ECHO 10
#define LEFT_TRIG 8
#define LEFT_ECHO 11
#define RIGHT_TRIG 9
#define RIGHT_ECHO 12
#define BACK_TRIG A2
#define BACK_ECHO 13

#define DISTANCE_THRESHOLD 7  // Threshold distance set to 7 cm

// Initialize LCD display (address 0x27 for a 16x2 LCD)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Function prototypes
void resetServos();  // Prototype for resetServos
void blinkLED(int ledPin);  // Prototype for blinkLED
void blinkBothLEDs(int leftPin, int rightPin);  // Prototype for blinking both LEDs

// Function to calculate distance using ultrasonic sensor
long readUltrasonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);  // Clear the trigPin condition
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  // Set the trigPin high for 10us
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);  // Read the echoPin, returns sound wave travel time in microseconds
  long distance = (duration / 2) / 29.1;  // Convert time to distance
  return distance;
}

void setup() {
  // Set up the ultrasonic sensor pins
  pinMode(FRONT_TRIG, OUTPUT);
  pinMode(FRONT_ECHO, INPUT);
  pinMode(LEFT_TRIG, OUTPUT);
  pinMode(LEFT_ECHO, INPUT);
  pinMode(RIGHT_TRIG, OUTPUT);
  pinMode(RIGHT_ECHO, INPUT);
  pinMode(BACK_TRIG, OUTPUT);
  pinMode(BACK_ECHO, INPUT);

  // Set LED pins as output
  pinMode(leftLED, OUTPUT);
  pinMode(rightLED, OUTPUT);
  pinMode(frontLED, OUTPUT);

  // Set Joystick pins as input
  pinMode(joyX, INPUT);
  pinMode(joyY, INPUT);

  // Servo initialization
  leftServo.attach(leftServoPin);
  rightServo.attach(rightServoPin);

  // Set servos to neutral positions initially
  resetServos();

  // Initialize the LCD
  lcd.begin(16, 2);  
  lcd.backlight();
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Car System Ready");
  delay(2000);  // Display welcome message for 2 seconds
  lcd.clear();
}

void loop() {
  long frontDist = readUltrasonicDistance(FRONT_TRIG, FRONT_ECHO);
  long leftDist = readUltrasonicDistance(LEFT_TRIG, LEFT_ECHO);
  long rightDist = readUltrasonicDistance(RIGHT_TRIG, RIGHT_ECHO);
  long backDist = readUltrasonicDistance(BACK_TRIG, BACK_ECHO);

  lcd.clear();  // Clear LCD to update new status

  // Check for obstacles in all directions
  bool obstacleDetected = false;

  if (frontDist < DISTANCE_THRESHOLD) {
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Front!");
    lcd.setCursor(0, 1);
    lcd.print("Car Stopped");
    resetServos();  // Stop the car
    obstacleDetected = true;
  } else if (leftDist < DISTANCE_THRESHOLD) {
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Left!");
    lcd.setCursor(0, 1);
    lcd.print("Car Stopped");
    resetServos();
    obstacleDetected = true;
  } else if (rightDist < DISTANCE_THRESHOLD) {
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Right!");
    lcd.setCursor(0, 1);
    lcd.print("Car Stopped");
    resetServos();
    obstacleDetected = true;
  } else if (backDist < DISTANCE_THRESHOLD) {
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Back!");
    lcd.setCursor(0, 1);
    lcd.print("Car Stopped");
    resetServos();
    obstacleDetected = true;
  } else {
    lcd.setCursor(0, 0);
    lcd.print("Clear Path");
    lcd.setCursor(0, 1);
    lcd.print("Moving Forward");
  }

  // If no obstacles are detected, use joystick to control the car
  if (!obstacleDetected) {
    // Read joystick values
    int xValue = analogRead(joyX);
    int yValue = analogRead(joyY);

    // Left turn: left servo turns and left LED blinks
    if (xValue < 400) {
      leftServo.write(45); // Adjust to the desired left turn angle
      digitalWrite(rightLED, LOW);
      blinkLED(leftLED);
    }
    // Right turn: right servo turns and right LED blinks
    else if (xValue > 600) {
      rightServo.write(135); // Adjust to the desired right turn angle
      digitalWrite(leftLED, LOW);
      blinkLED(rightLED);
    } else {
      // Reset steering servos when the joystick is in the neutral X position
      leftServo.write(neutralTurnAngle);
      rightServo.write(neutralTurnAngle);
      digitalWrite(leftLED, LOW);
      digitalWrite(rightLED, LOW);
    }

    // Forward movement: forward servo turns and front LED blinks
    if (yValue > 600) {
      blinkLED(frontLED);
    }
    // Backward movement: both left and right LEDs blink
    else if (yValue < 400) {
      blinkBothLEDs(leftLED, rightLED);
    } else {
      digitalWrite(frontLED, LOW);
    }
  }

  delay(50);  // Small delay for stability
}

// Function to blink a single LED
void blinkLED(int ledPin) {
  digitalWrite(ledPin, HIGH);
  delay(300);  // Reduced delay for faster blinking
  digitalWrite(ledPin, LOW);
  delay(150);
}

// Function to blink both LEDs together
void blinkBothLEDs(int leftPin, int rightPin) {
  digitalWrite(leftPin, HIGH);
  digitalWrite(rightPin, HIGH);
  delay(150);
  digitalWrite(leftPin, LOW);
  digitalWrite(rightPin, LOW);
  delay(150);
}

// Function to reset all servos to neutral positions
void resetServos() {
  leftServo.write(neutralTurnAngle);  // Reset left servo to neutral (center) position
  rightServo.write(neutralTurnAngle); // Reset right servo to neutral (center) position
}