#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 


int duration;  
int distance;  
int angle;  


const char* ledStatus[] = {"Red   ", "Purple  ", "Green ", "None "};  


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





void setup() {
  initPins();  
  initLCD();  
  initServo();  
}



void loop() {
  distance = getDistance();  
  displayDistance(distance);  
  updateLEDsAndServo(distance);  
  displayAngle(angle);  
  delay(200);  
}






void initPins() {
  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);  
}



void initLCD() {
  lcd.init();  
  lcd.backlight();  
}



void initServo() {
  servoMotor.attach(SERVO_PIN);  
}




int getDistance() {
  digitalWrite(TRIG_PIN, HIGH);  
  delayMicroseconds(10);  
  digitalWrite(TRIG_PIN, LOW); 

  duration = pulseIn(ECHO_PIN, HIGH);  
  return duration  / 58;  
}



void displayDistance(int dist) {
  float dist_m = dist / 100.0;
  lcd.setCursor(0, 0);  
  lcd.print("Distance: ");  
  lcd.print(dist);  
  lcd.print(" cm ");  

  lcd.setCursor(0, 1); 
  lcd.print("          ");
  lcd.print(dist_m);
  lcd.print(" m ");
}



void displayAngle(int ang) {
  lcd.setCursor(0, 2);  
  lcd.print("Angle: ");  
  lcd.print(ang);  
  lcd.print(" deg ");  
}





void updateLEDsAndServo(int dist) {
  int ledIndex = 3;  
  if (dist >= 0 && dist <= 20) {
    digitalWrite(RED_LED_PIN, HIGH);  
    digitalWrite(BLUE_LED_PIN, LOW);  
    digitalWrite(GREEN_LED_PIN, LOW);  
    angle = 180;  
    ledIndex = 0;  
  } else if (dist >= 21 && dist <= 40) {
    digitalWrite(RED_LED_PIN, HIGH);  
    digitalWrite(BLUE_LED_PIN, HIGH);  
    digitalWrite(GREEN_LED_PIN, LOW);  
    angle = 90;  
    ledIndex = 1; 
  } else if (dist >= 41 && dist <= 100) {
    digitalWrite(RED_LED_PIN, LOW);  
    digitalWrite(BLUE_LED_PIN, LOW);  
    digitalWrite(GREEN_LED_PIN, HIGH);  
    angle = 45;  
    ledIndex = 2;  
  } else {
    digitalWrite(RED_LED_PIN, LOW);  
    digitalWrite(BLUE_LED_PIN, LOW);  
    digitalWrite(GREEN_LED_PIN, LOW);  
    angle = 0;  
  }

  servoMotor.write(angle);  

  lcd.setCursor(0, 3);  
  lcd.print("LED: ");  
  lcd.print(ledStatus[ledIndex]);  
}