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

LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo servoMotor1; // First servo motor
Servo servoMotor2; // Second servo motor

// Pins for Ultrasonic Sensors
const int trigFront = 2;
const int echoFront = 3;
const int trigRight = 4;
const int echoRight = 5;
const int trigLeft = 6;
const int echoLeft = 7; 
const int trigBack = 8; 
const int echoBack = 9; 

#define leftLED 12
#define rightLED 13
#define idleLED 16

// Servo Pins
const int servoPin1 = 10; // Pin for controlling the first servo
const int servoPin2 = 11; // Pin for controlling the second servo

// Joystick Pin
const int joystickY = A0; // Y-axis of the joystick (for forward/backward control)
const int joystickX = A1; // X-axis of the Joystick (for left/right control)

// Threshold distance in centimeters
const int threshold = 10; // Adjust based on your needs

void setup() {
  Serial.begin(9600);
  lcd.begin(20, 4);
  lcd.backlight();

  // Initialize ultrasonic sensor pins
  pinMode(trigFront, OUTPUT);
  pinMode(echoFront, INPUT);
  pinMode(trigRight, OUTPUT);
  pinMode(echoRight, INPUT);
  pinMode(trigLeft, OUTPUT);
  pinMode(echoLeft, INPUT);
  pinMode(trigBack, OUTPUT);
  pinMode(echoBack, INPUT);
  pinMode(leftLED, OUTPUT);
  pinMode(rightLED, OUTPUT);
  pinMode(idleLED, OUTPUT);

  // Attach the servos to their pins
  servoMotor1.attach(servoPin1);
  servoMotor2.attach(servoPin2);

  lcd.setCursor(0, 0);
  lcd.print("Setup complete.");
  delay(1000);
}

void loop() {
  // Measure distances from all sensors
  int frontDist = measureDistance(trigFront, echoFront);
  int rightDist = measureDistance(trigRight, echoRight);
  int leftDist = measureDistance(trigLeft, echoLeft);
  int backDist = measureDistance(trigBack, echoBack);

  Serial.print("Front: ");
  Serial.print(frontDist);
  Serial.print(" cm, Left: ");
  Serial.print(leftDist);
  Serial.print(" cm, Right: ");
  Serial.print(rightDist);
  Serial.print(" cm, Back: ");
  Serial.print(backDist);
  Serial.println(" cm");

  // Read joystick values
  int joystickYValue = analogRead(joystickY);
  int joystickXValue = analogRead(joystickX);

  // Obstacle detection and display on LCD
  if (frontDist < threshold) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle: Front");
    lcd.setCursor(0, 2);
    lcd.print("Move: Left, Right or");
    lcd.setCursor(0, 3);
    lcd.print("Backward");
  } else if (backDist < threshold) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle: Back");
    lcd.setCursor(0, 2);
    lcd.print("Move: Left, Right or");
    lcd.setCursor(0, 3);
    lcd.print("Forward");
  } else if (rightDist < threshold) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle: Right");
    lcd.setCursor(0, 2);
    lcd.print("Move: Fwd, Bwd or");
    lcd.setCursor(0, 3);
    lcd.print("Left");    
  } else if (leftDist < threshold) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle: Left");
    lcd.setCursor(0, 2);
    lcd.print("Move: Fwd, Bwd or");
    lcd.setCursor(0, 3);
    lcd.print("Right");
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("No Obstacle");
  }

  // Control movement based on joystick and obstacle detection
  controlMovement(joystickYValue, joystickXValue, frontDist, backDist, leftDist, rightDist);

  delay(200);
}

// Function to measure distance using an ultrasonic sensor
int measureDistance(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.034 / 2; // Convert to cm
  return distance;
}

// Function to control movement based on joystick input and obstacle detection
void controlMovement(int joystickYValue, int joystickXValue, int frontDist, int backDist, int leftDist, int rightDist) {
  // Move forward only if front is clear
  if (joystickYValue < 400 && frontDist >= threshold) {  
    moveForward(map(joystickYValue, 400, 0, 90, 180));
  }
  // Move backward only if back is clear
  else if (joystickYValue > 600 && backDist >= threshold) {  
    moveBackward(map(joystickYValue, 600, 1023, 90, 0));
  }
  // Move left only if left is clear
  else if (joystickXValue < 400 && leftDist >= threshold) {  
    moveLeft(map(joystickXValue, 400, 0, 90, 180));
  }
  // Move right only if right is clear
  else if (joystickXValue > 600 && rightDist >= threshold) {  
    moveRight(map(joystickXValue, 600, 1023, 90, 0));
  }
  // Stop if no movement is allowed
  else {
    stopMotors();
  }
}

// Function to move both servos forward
void moveForward(int angle) {
  servoMotor1.write(angle);
  Serial.println("Moving forward");
  lcd.setCursor(0, 1);
  lcd.print("Moving forward");
  digitalWrite(idleLED, HIGH);  
}

// Function to move both servos backward
void moveBackward(int angle) {
  servoMotor1.write(angle);
  Serial.println("Moving backward");
  lcd.setCursor(0, 1);
  lcd.print("Moving backward");
  digitalWrite(idleLED, HIGH);
}

// Function to move the second servo to the right
void moveRight(int angle) {
  servoMotor2.write(angle);
  Serial.println("Moving Right");
  lcd.setCursor(0, 1);
  lcd.print("Moving Right");
  digitalWrite(idleLED, HIGH);
  digitalWrite(rightLED, HIGH);
  delay(200);
  digitalWrite(rightLED, LOW);
  delay(200);
}

// Function to move the second servo to the left
void moveLeft(int angle) {
  servoMotor2.write(angle);
  Serial.println("Moving Left");
  lcd.setCursor(0, 1);
  lcd.print("Moving Left");
  digitalWrite(idleLED, HIGH);
  digitalWrite(leftLED, HIGH);
  delay(200);
  digitalWrite(leftLED, LOW);
  delay(200);
}

// Function to stop both servos
void stopMotors() {
  servoMotor1.write(90);
  servoMotor2.write(90);
  Serial.println("Motors stopped.");
  lcd.setCursor(0, 1);
  lcd.print("Motors stopped");
  digitalWrite(idleLED, HIGH);
}