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

#define TRIG_PIN 7
#define ECHO_PIN 6
#define SERVO_PIN 9

LiquidCrystal_I2C lcd(0x27, 20, 4); // Set the LCD I2C address to 0x27 for a 20x4 LCD
Servo myServo;

long duration;
int distance;
int currentAngle = 0;  // Initialize servo position

void setup() {
  lcd.init();
  lcd.backlight();
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  myServo.attach(SERVO_PIN);
  myServo.write(currentAngle);  // Initialize servo to 0
  
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
}

void loop() {
  // Trigger the HC-SR04 sensor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  // Read the Echo pin
  duration = pulseIn(ECHO_PIN, HIGH);
  
  // Calculate the distance in cm
  distance = duration * 0.034 / 2;
  
  // Display the distance on the LCD
  lcd.setCursor(0, 1);
  lcd.print("                "); // Clear previous data
  lcd.setCursor(0, 1);
  lcd.print(distance);
  lcd.print(" cm");
  
  // Control the servo motor based on distance
  if (distance > 100) {
    smoothServoMove(180);
  } else {
    smoothServoMove(0);
  }

  delay(500); // Refresh every 500ms
}

void smoothServoMove(int targetAngle) {
  if (currentAngle < targetAngle) {
    for (int angle = currentAngle; angle <= targetAngle; angle++) {
      myServo.write(angle);
      delay(15);  // Adjust this to control speed
    }
  } else if (currentAngle > targetAngle) {
    for (int angle = currentAngle; angle >= targetAngle; angle--) {
      myServo.write(angle);
      delay(15);  // Adjust this to control speed
    }
  }
  currentAngle = targetAngle; // Update current angle
}