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

// Define pins
#define TRIG_PIN 9
#define ECHO_PIN 10
#define GREEN_LED_PIN 12
#define RED_LED_PIN 13
#define BUZZER_PIN 8
#define SERVO_PIN 6

// Initialize objects
LiquidCrystal_I2C lcd(0x3F, 16, 2);  // LCD I2C address 0x3F, 16 columns, 2 rows
Servo myServo;  // Servo motor object

void setup() {
  // Start the LCD
  lcd.begin(16, 2);
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  
  // Initialize pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  
  myServo.attach(SERVO_PIN);  // Attach servo to pin 6
}

void loop() {
  long duration;
  int distance;
  
  // Trigger the ultrasonic sensor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  // Read the echo time
  duration = pulseIn(ECHO_PIN, HIGH);
  
  // Calculate the distance
  distance = duration * 0.034 / 2;
  
  // Display the distance on the LCD
  lcd.setCursor(0, 1);
  lcd.print("Dist: ");
  lcd.print(distance);
  lcd.print(" cm");
  
  // LED and Buzzer Logic
  if (distance < 20) {  // Collision risk zone (less than 20 cm)
    digitalWrite(GREEN_LED_PIN, LOW);  // Turn off green LED
    digitalWrite(RED_LED_PIN, HIGH);   // Turn on red LED
    digitalWrite(BUZZER_PIN, HIGH);    // Sound the buzzer
    myServo.write(0);  // Turn servo to simulate collision avoidance
  } else {
    digitalWrite(GREEN_LED_PIN, HIGH); // Turn on green LED
    digitalWrite(RED_LED_PIN, LOW);    // Turn off red LED
    digitalWrite(BUZZER_PIN, LOW);     // Turn off buzzer
    myServo.write(90); // Keep servo in default position
  }
  
  delay(500);  // Small delay to stabilize the readings
}