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

LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo Motor pins
Servo leftServo;
Servo rightServo;
Servo servo1;
Servo servo2;

// Pin definitions for joystick and servos
#define SERVO1_PIN A3
#define SERVO2_PIN A2
#define JOY_X A1
#define JOY_Y A0

// Ultrasonic sensor pins
#define FRONT_TRIG 13
#define FRONT_ECHO 12
#define BACK_TRIG 10
#define BACK_ECHO 9
#define LEFT_TRIG 7
#define LEFT_ECHO 6
#define RIGHT_TRIG 4
#define RIGHT_ECHO 3

// LED Pins
#define LED_FRONT 11
#define LED_BACK 8
#define LED_LEFT 5
#define LED_RIGHT 2

// Constants for ultrasonic sensor distance calculation
#define MAX_DISTANCE 200
#define OBSTACLE_DISTANCE 30 

// Create NewPing objects for each ultrasonic sensor
NewPing sonarFront(FRONT_TRIG, FRONT_ECHO, MAX_DISTANCE);
NewPing sonarBack(BACK_TRIG, BACK_ECHO, MAX_DISTANCE);
NewPing sonarLeft(LEFT_TRIG, LEFT_ECHO, MAX_DISTANCE);
NewPing sonarRight(RIGHT_TRIG, RIGHT_ECHO, MAX_DISTANCE);

void setup() {
  lcd.init();
  lcd.backlight();
  
  // Initialize servo motors
  leftServo.attach(SERVO1_PIN);
  rightServo.attach(SERVO2_PIN);
  servo1.attach(A3); 
  servo2.attach(A2);
  
  // Initialize LEDs as outputs
  pinMode(LED_FRONT, OUTPUT);
  pinMode(LED_BACK, OUTPUT);
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);

}

void loop() {
  // Read distance from each ultrasonic sensor
  int distanceFront = sonarFront.ping_cm();
  int distanceBack = sonarBack.ping_cm();
  int distanceLeft = sonarLeft.ping_cm();
  int distanceRight = sonarRight.ping_cm();

  displayDistances(distanceFront, distanceBack, distanceLeft, distanceRight);

  checkForObstacles(distanceFront, distanceBack, distanceLeft, distanceRight);

  // If no obstacles detected, car can move based on joystick input
  if (distanceFront > OBSTACLE_DISTANCE && distanceBack > OBSTACLE_DISTANCE &&
      distanceLeft > OBSTACLE_DISTANCE && distanceRight > OBSTACLE_DISTANCE) {
    moveCarWithJoystick();
  }
  
  delay(100); 
}

void displayDistances(int distanceFront, int distanceBack, int distanceLeft, int distanceRight) {
  lcd.clear();

  // Display front distance
  lcd.setCursor(0, 0);
  lcd.print("Front: ");
  lcd.print(distanceFront);
  lcd.print(" cm");

  // Display back distance on the second row
  lcd.setCursor(0, 1);
  lcd.print("Back: ");
  lcd.print(distanceBack);
  lcd.print(" cm");

  delay(1000); 

  // Display left and right distances on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Left: ");
  lcd.print(distanceLeft);
  lcd.print(" cm");

  lcd.setCursor(0, 1);
  lcd.print("Right: ");
  lcd.print(distanceRight);
  lcd.print(" cm");

  delay(1000); 
}

void checkForObstacles(int distanceFront, int distanceBack, int distanceLeft, int distanceRight) {

  if (distanceFront > 0 && distanceFront <= OBSTACLE_DISTANCE) {
          blinkLED(LED_FRONT); 
          haltCar();
        } 
            else {
              digitalWrite(LED_FRONT, LOW);
            }

  if (distanceBack > 0 && distanceBack <= OBSTACLE_DISTANCE) {
          blinkLED(LED_BACK);   
          haltCar();
        } 
            else {
              digitalWrite(LED_BACK, LOW);  
            }

  if (distanceLeft > 0 && distanceLeft <= OBSTACLE_DISTANCE) {
          blinkLED(LED_LEFT);   
          moveServo2();         
          haltCar();
        } 
            else {
              digitalWrite(LED_LEFT, LOW); 
            }

  if (distanceRight > 0 && distanceRight <= OBSTACLE_DISTANCE) {
          blinkLED(LED_RIGHT);
          moveServo1(); 
          haltCar();
        } 
            else {
              digitalWrite(LED_RIGHT, LOW); 
            }
}

void blinkLED(int ledPin) {
  digitalWrite(ledPin, HIGH);  
  delay(100);                  
  digitalWrite(ledPin, LOW);   
  delay(100);                 
}

// Function to move Servo1 to 180 degrees when right sensor detects an object
void moveServo1() {
  servo1.write(180);   
  delay(500);
}

// Function to move Servo2 to 0 degrees when left sensor detects an object
void moveServo2() {
  servo2.write(0);  
  delay(500);
}

void haltCar() {
  leftServo.write(90);  
  rightServo.write(90);
}

void moveCarWithJoystick() {
  int joystickX = analogRead(JOY_X);
  int joystickY = analogRead(JOY_Y);

  if (joystickY > 600) {
    leftServo.write(180); 
    rightServo.write(0); 
    lcd.setCursor(0, 0);
    lcd.print("Moving Forward ");
  } 
  
          else if (joystickY < 400) {
            leftServo.write(0);  
            rightServo.write(180);
            lcd.setCursor(0, 0);
            lcd.print("Moving Backward");
          } 
          
          else if (joystickX > 600) {
            leftServo.write(180); 
            rightServo.write(90);  
            lcd.setCursor(0, 0);
            lcd.print("Turning Right   ");
            moveServo1();        
          } 
          
          else if (joystickX < 400) {
            leftServo.write(90);  
            rightServo.write(180); 
            lcd.setCursor(0, 0);
            lcd.print("Turning Left    ");
            moveServo2();   
          } 
  
  else {
    haltCar();
    lcd.setCursor(0, 0);
    lcd.print("Car Stopped     ");
  }

}