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

// กำหนดพอร์ต I2C สำหรับ LCD2004
LiquidCrystal_I2C lcd(0x27, 20, 4);

// กำหนดพอร์ตสำหรับเซ็นเซอร์ HC-SR04
const int trigPin = 7;
const int echoPin = 6;

// กำหนดพอร์ตสำหรับเซอร์โวมอเตอร์
Servo myServo;
const int servoPin = 9;

// ฟังก์ชันวัดระยะทางจาก HC-SR04
long getDistance() {
  // กำหนดขา trig เป็น low เพื่อเคลียร์สถานะก่อนส่งสัญญาณ
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // ส่งสัญญาณ trig
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // รับสัญญาณจาก echo และคำนวณระยะทาง
  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.034 / 2;  // คำนวณระยะทางเป็น cm
  
  return distance;
}

// ฟังก์ชันแสดงผลบน LCD
void displayDistance(long distance) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm");
}

// ฟังก์ชันควบคุมการหมุนของเซอร์โว
void controlServo(long distance) {
  int angle;
  
  // กำหนดมุมหมุนตามระยะทาง
  if (distance < 100) {
    angle = 180;
  } else {
    angle = 0;
  }
  
  // หมุนเซอร์โวอย่างนุ่มนวล
  int currentAngle = myServo.read();
  if (currentAngle < angle) {
    for (int pos = currentAngle; pos <= angle; pos++) {
      myServo.write(pos);
      delay(15);  // หน่วงเวลาเพื่อให้การหมุนเป็นไปอย่างนุ่มนวล
    }
  } else {
    for (int pos = currentAngle; pos >= angle; pos--) {
      myServo.write(pos);
      delay(15);
    }
  }
}

void setup() {
  // เริ่มต้นการทำงานของ LCD
  lcd.init();
  lcd.backlight();
  
  // กำหนดพอร์ตสำหรับ HC-SR04
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // เริ่มต้นการทำงานของเซอร์โว
  myServo.attach(servoPin);
  myServo.write(0);  // เริ่มต้นที่มุม 0 องศา
  
  // แสดงข้อความเริ่มต้นบน LCD
  lcd.setCursor(0, 0);
  lcd.print("Initializing...");
  delay(1000);
}

void loop() {
  // อ่านค่าระยะทางจากเซ็นเซอร์
  long distance = getDistance();
  
  // แสดงผลระยะทางบน LCD
  displayDistance(distance);
  
  // ควบคุมการหมุนของเซอร์โวตามระยะทาง
  controlServo(distance);
  
  delay(500);  // หน่วงเวลาเล็กน้อยก่อนทำซ้ำ
}