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

#define TRIG_PIN 5
#define ECHO_PIN 4
#define MOTOR_PIN 13
#define RGB_RED_PIN 9
#define RGB_GREEN_PIN 16
#define DISTANCE_THRESHOLD 50

Servo motor;
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(115200);
   Wire.begin(21, 20); 
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RGB_RED_PIN, OUTPUT);
  pinMode(RGB_GREEN_PIN, OUTPUT);
  motor.attach(MOTOR_PIN);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");
  delay(2000);
  lcd.clear();
  motor.write(0); 
  lcd.setCursor(0, 0);
  lcd.print("Gate Opened");
  Serial.println("Gate Opened");
  delay(2000);

  motor.write(90); 
  lcd.setCursor(0, 0);
  lcd.print("Gate Closed");
  Serial.println("Gate Closed");
  delay(2000);
  lcd.clear();
}

void loop() {
  long distance = measureDistance();
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm   "); 

  if (distance < DISTANCE_THRESHOLD) {
    setRGBColor(0, 255); 
  } else {
    setRGBColor(255, 0); 
  }
  delay(500);
}
long measureDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  return (duration * 0.034) / 2; 
}

void setRGBColor(int red, int green) {
  analogWrite(RGB_RED_PIN, red);
  analogWrite(RGB_GREEN_PIN, green);
}