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

// Pins configuration
const int TRIG_PIN = 2;
const int ECHO_PIN = 3;
const int SERVO_PIN = 11;

// LCD setup (0x27 or 0x3F I2C address, 16x2 display)
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo directionServo;

void setup() {
  // Initialize components
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  directionServo.attach(SERVO_PIN);
  
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Direction:");
  lcd.setCursor(0, 1);
  lcd.print("Distance: ");
}

void loop() {
  // Sweep from 0° to 180°
  for (int angle = 0; angle <= 180; angle += 45) {
    updateDirection(angle);
    delay(500);
  }
  
  // Sweep from 180° to 0°
  for (int angle = 180; angle >= 0; angle -= 45) {
    updateDirection(angle);
    delay(500);
  }
}

void updateDirection(int angle) {
  // Move servo to specified angle
  directionServo.write(angle);
  
  // Measure distance
  float distance = getDistance();
  
  // Update LCD display
  lcd.setCursor(10, 0);
  lcd.print("    ");
  lcd.setCursor(10, 0);
  
  // Print direction based on angle
  if (angle < 60) lcd.print("Right ");
  else if (angle < 120) lcd.print("Front ");
  else lcd.print("Left  ");
  
  lcd.setCursor(9, 1);
  lcd.print("       ");
  lcd.setCursor(9, 1);
  
  if (distance >= 400 || distance <= 2) {
    lcd.print("ERROR");
  } else {
    lcd.print(distance, 1);
    lcd.print(" cm");
  }
}

float getDistance() {
  // Send ultrasonic pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  // Measure echo duration
  long duration = pulseIn(ECHO_PIN, HIGH);
  
  // Calculate distance in cm
  return duration * 0.034 / 2;
}