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

Servo leftServo;    
Servo rightServo;   
Servo forwardServo; 
Servo backwardServo; 

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int trigPinFront = 13;
const int echoPinFront = 12;
const int trigPinLeft = 11;
const int echoPinLeft = 10;
const int trigPinRight = 9;
const int echoPinRight = 8;
const int trigPinBack = 7;
const int echoPinBack = 6;

int x_key = A0;  
int y_key = A1; 

int x_pos;  
int y_pos;  

int leftServo_pin = 2;    
int rightServo_pin = 3;   
int forwardServo_pin = 4;  
int backwardServo_pin = 5; 

int initial_leftServoPos = 90;    
int initial_rightServoPos = 90;   
int initial_forwardServoPos = 90; 
int initial_backwardServoPos = 90; 

long duration;
int distanceFront, distanceLeft, distanceRight, distanceBack;

int getDistance(int trigPin, int echoPin);
void haltCar();
void moveForward();
void moveBackward();

void setup() {
  lcd.init();
  lcd.backlight();

  pinMode(trigPinFront, OUTPUT);
  pinMode(echoPinFront, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);
  pinMode(trigPinBack, OUTPUT);
  pinMode(echoPinBack, INPUT);

  leftServo.attach(leftServo_pin);   
  rightServo.attach(rightServo_pin); 
  forwardServo.attach(forwardServo_pin); 
  backwardServo.attach(backwardServo_pin); 

  leftServo.write(initial_leftServoPos);  
  rightServo.write(initial_rightServoPos); 
  forwardServo.write(initial_forwardServoPos); 
  backwardServo.write(initial_backwardServoPos);

  Serial.begin(9600);  
  pinMode(x_key, INPUT); 
  pinMode(y_key, INPUT);  
}

void loop() {
  x_pos = analogRead(x_key);  
  y_pos = analogRead(y_key);  

  if (x_pos > 600) {
    forwardServo.write(0);    
    backwardServo.write(0);   
  } else if (x_pos < 400) {
    forwardServo.write(180);  
    backwardServo.write(180); 
  } else {
    forwardServo.write(90);  
    backwardServo.write(90);  
  }

  if (y_pos > 600) {
    leftServo.write(0);   
    rightServo.write(0); 
  } else if (y_pos < 400) {
    leftServo.write(180);  
    rightServo.write(180);   
  } else {
    leftServo.write(90);   
    rightServo.write(90);  
  }

  distanceFront = getDistance(trigPinFront, echoPinFront);
  distanceLeft = getDistance(trigPinLeft, echoPinLeft);
  distanceRight = getDistance(trigPinRight, echoPinRight);
  distanceBack = getDistance(trigPinBack, echoPinBack);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("F:"); lcd.print(distanceFront);
  lcd.print(" L:"); lcd.print(distanceLeft);
  lcd.setCursor(0, 1);
  lcd.print("R:"); lcd.print(distanceRight);
  lcd.print(" B:"); lcd.print(distanceBack);

  if (distanceFront < 10) {
    haltCar();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Ahead!");
    lcd.setCursor(0, 1);
    lcd.print("STOP! GO BACKWARD");
    delay(2000);
  } else if (distanceLeft < 10) {
    haltCar();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Left!");
    lcd.setCursor(0, 1);
    lcd.print("STOP! TURN RIGHT!");
    delay(2000);
  } else if (distanceRight < 10) {
    haltCar();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Right!");
    lcd.setCursor(0, 1);
    lcd.print("STOP! TURN LEFT!");
    delay(2000);
  } else if (distanceBack < 10) {
    haltCar();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Obstacle Behind!");
    lcd.setCursor(0, 1);
    lcd.print("STOP! GO FORWARD");
    delay(2000);
  } else {
    delay(100);  
  }
}

int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;  
  return distance;
}

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

void moveForward() {
  leftServo.write(0);    
  rightServo.write(0);   
}

void moveBackward() {
  leftServo.write(180);  
  rightServo.write(180); 
}