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

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

// Servo Setup
Servo servo1; 
Servo servo2;  

// Pin Assignments
int JS_X = A1;       // Joystick X-axis
int JS_Y = A2;       // Joystick Y-axis
int BP = A0;         // Joystick Button
int LLP = 2;         // Left LED
int CLP = 4;         // Center LED
int RLP = 3;         // Right LED
int servo1_pin = 8;  // Servo 1 (left-right)
int servo2_pin = 9;  // Servo 2 (up-down)

// Ultrasonic Sensors Pins (Trig, Echo)
const int trigPinFront = 10;
const int echoPinFront = 11;
const int trigPinLeft = 12;
const int echoPinLeft = 13;
const int trigPinRight = 6;
const int echoPinRight = 7;
const int trigPinBack = 5;
const int echoPinBack = 1;

// Variables for Joystick and Servo
int x_pos, y_pos;
int servo1_position = 90; // Initial position for Servo 1
int servo2_position = 90; // Initial position for Servo 2

// Sensor variables
long durationFront, distanceFront;
long durationLeft, distanceLeft;
long durationRight, distanceRight;
long durationBack, distanceBack;

const long blinkInterval = 500; // Time interval for LED blink
unsigned long lastBlinkTime = 0;
bool ledState = LOW;

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.backlight();
  
  // Initialize servos
  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  servo1.write(servo1_position);  
  servo2.write(servo2_position);

  // Initialize Joystick and LEDs
  pinMode(JS_X, INPUT);
  pinMode(JS_Y, INPUT);
  pinMode(BP, INPUT_PULLUP); 
  pinMode(LLP, OUTPUT);
  pinMode(CLP, OUTPUT);
  pinMode(RLP, OUTPUT);

  // Initialize Ultrasonic Sensors
  pinMode(trigPinFront, OUTPUT);
  pinMode(echoPinFront, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(trigPinBack, OUTPUT);
  pinMode(echoPinBack, INPUT);
}

void loop() {
  // Read Joystick X and Y values
  x_pos = analogRead(JS_X);
  y_pos = analogRead(JS_Y);

  // Control Servo1 (left-right) using Joystick X
  if (x_pos < 300) {
    blinkLED(LLP); // Left LED blink
    servo1_position = max(0, servo1_position + 20);
    servo1.write(servo1_position);
  } else if (x_pos > 700) {
    blinkLED(RLP); // Right LED blink
    servo1_position = min(180, servo1_position - 20);
    servo1.write(servo1_position);
  } else {
    // Turn off both left and right LEDs when joystick is in the center
    digitalWrite(LLP, LOW);
    digitalWrite(RLP, LOW);
    servo1.write(90);  
    servo2.write(90);
  }

  // Control Servo2 (up-down) using Joystick Y
  if (y_pos < 300) {
    servo2_position = max(0, servo2_position + 20);
    servo2.write(servo2_position);
  } else if (y_pos > 700) {
    servo2_position = min(180, servo2_position - 20);
    servo2.write(servo2_position);
  }

  // Read distances from ultrasonic sensors
  distanceFront = getDistance(trigPinFront, echoPinFront);
  distanceLeft = getDistance(trigPinLeft, echoPinLeft);
  distanceRight = getDistance(trigPinRight, echoPinRight);
  distanceBack = getDistance(trigPinBack, echoPinBack);

  // Check for obstacles and take actions
  if (distanceFront < 20) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("FRONT OBS");
    lcd.setCursor(0, 1);
    lcd.print("GO L R D");
    haltCar();
    digitalWrite(CLP, HIGH); // Turn on Center LED when obstacle detected
  } 

  if (distanceLeft < 20) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("LEFT OBS");
    lcd.setCursor(0, 1);
    lcd.print("GO U R D");
    haltCar();
    digitalWrite(CLP, HIGH); // Turn on Center LED when obstacle detected
  } 

  if (distanceRight < 20) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("RIGHT OBS");
    lcd.setCursor(0, 1);
    lcd.print("GO L U D");
    haltCar();
    digitalWrite(CLP, HIGH); // Turn on Center LED when obstacle detected
  } 

  if (distanceBack < 20) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("BACK OBS");
    lcd.setCursor(0, 1);
    lcd.print("GO F R L");
    haltCar();
    digitalWrite(CLP, HIGH); // Turn on Center LED when obstacle detected
  } 
  else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("All Clear");
    digitalWrite(CLP, LOW); // Turn off Center LED when no obstacle
  }
  delay(500);
}

// Function to measure distance from an ultrasonic sensor
long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration * 0.034) / 2; // Distance in cm
  return distance;
}

// Function to halt the car (stops servo motors)
void haltCar() {
  servo1.write(90);  // Stop movement by resetting servos
  servo2.write(90);
  delay(1000);  // Wait for 1 second
}

// Function to blink the LED
void blinkLED(int ledPin) {
  unsigned long currentMillis = millis();
  if (currentMillis - lastBlinkTime >= blinkInterval) {
    lastBlinkTime = currentMillis;
    ledState = !ledState;
    digitalWrite(ledPin, ledState);
  }
}