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

#define TRIG_PIN 5
#define ECHO_PIN 6
#define RED_LED_PIN 2
#define BLUE_LED_PIN 4
#define GREEN_LED_PIN 3
#define SERVO_PIN 9



Servo servoMotor;
LiquidCrystal_I2C lcd(0x27, 20, 4);

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(BLUE_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  Serial.begin(9600);

  lcd.init();
  lcd.backlight();

  servoMotor.attach(SERVO_PIN);
}

long getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  return pulseIn(ECHO_PIN, HIGH) / 58.2;
}

void displayInfo(long distance, int angle, int ledIndex) {
  lcd.setCursor(0, 0); lcd.print("Dist: "); lcd.print(distance); lcd.print(" cm    ");
  lcd.setCursor(0, 1); lcd.print("Ang: "); lcd.print(angle); lcd.print(" deg     ");
 
}

void setLEDs(bool red, bool blue, bool green) {
  digitalWrite(RED_LED_PIN, red);
  digitalWrite(BLUE_LED_PIN, blue);
  digitalWrite(GREEN_LED_PIN, green);
}

void loop() {
  long distance = getDistance();
  int angle, ledIndex;

  if (distance <= 20) {
    setLEDs(true, false, false);
    angle = 180; ledIndex = 0;
  } else if (distance <= 40) {
    setLEDs(true, true, false);
    angle = 90; ledIndex = 1;
  } else if (distance <= 100) {
    setLEDs(false, false, true);
    angle = 45; ledIndex = 2;
  } else {
    setLEDs(false, false, false);
    angle = 0; ledIndex = 3;
  }

  servoMotor.write(angle);
  displayInfo(distance, angle, ledIndex);
  delay(200);
}