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

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define PIN_TRIG 3 
#define PIN_ECHO 2

#define RED_PIN 6
#define GREEN_PIN 5
#define BLUE_PIN 4

Servo myServo; 
int servoPosition = 0; 
bool movingForward = true; 

void setup() {
  lcd.begin(16, 2); 
  lcd.backlight(); 

  Serial.begin(115200);

  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  myServo.attach(7); 
}

void loop() {
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  int duration = pulseIn(PIN_ECHO, HIGH);
  float distanceCM = duration / 58.0;
  
  float distanceInches = distanceCM / 2.54; 
  float distanceMM = distanceCM * 10; 

  Serial.print("Distance in CM: ");
  Serial.println(distanceCM);
  
  Serial.print("Distance in Inches: ");
  Serial.println(distanceInches);
  
  Serial.print("Distance in MM: ");
  Serial.println(distanceMM);

  lcd.clear(); 
  lcd.setCursor(0, 0); 
  lcd.print("Dist MM: ");
  lcd.print(distanceCM); 

  lcd.setCursor(0, 1); 
  lcd.print("Dist In: ");
  lcd.print(distanceInches); 
  
  delay(100); 

  if (distanceCM >= 2 && distanceCM <= 100) {
    setColor(0, 255, 0); 
  } else if (distanceCM >= 101 && distanceCM <= 200) {
    setColor(0, 0, 255); 
  } else if (distanceCM >= 201 && distanceCM <= 300) {
    setColor(255, 0, 0); 
  } else if (distanceCM > 300) {
    setColor(128, 0, 128); 
    moveServo(); 
  } else {
    setColor(0, 0, 0); 
  }
}

void setColor(int red, int green, int blue) {
  analogWrite(RED_PIN, red);
  analogWrite(GREEN_PIN, green);
  analogWrite(BLUE_PIN, blue);
}

void moveServo() {
    if (movingForward) {
        servoPosition++;
        if (servoPosition >= 180) {
            movingForward = false; 
        }
    } else {
        servoPosition--;
        if (servoPosition <= 0) {
            movingForward = true; 
        }
    }
    myServo.write(servoPosition);
    delay(15); 
}