#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h> // ไลบรารีควบคุม Servo

// สร้างออบเจ็กต์ LCD
LiquidCrystal_I2C lcd(0x27, 20, 4); // ที่อยู่ I2C ของ LCD อาจแตกต่างกัน (ลอง 0x3F หาก 0x27 ไม่ทำงาน)

// กำหนดขาของ HCSR04
const int trigPin = 5; 
const int echoPin = 6; 

// สร้างออบเจ็กต์ Servo
Servo myServo;
const int servoPin = 9; // ขาที่ใช้ควบคุม Servo

void setup() {
  lcd.init(); // เริ่มต้นการทำงานของ LCD
  lcd.backlight(); // เปิด backlight ของ LCD
  lcd.setCursor(0, 0); // ตั้งตำแหน่งการพิมพ์ที่บรรทัดแรก
  lcd.print("Distance: ");
  
  pinMode(trigPin, OUTPUT); // กำหนดขา Trig เป็น Output
  pinMode(echoPin, INPUT);  // กำหนดขา Echo เป็น Input
  Serial.begin(9600); // เปิด Serial monitor สำหรับดูข้อมูล

  myServo.attach(servoPin); // เชื่อมต่อ Servo เข้ากับขา 9
  myServo.write(0); // เริ่มต้นที่มุม 0 องศา
}

void loop() {
  long duration;
  float distanceCm;

  // สร้าง pulse ที่ขา Trig
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // อ่านระยะเวลาที่ใช้ในการสะท้อนกลับของคลื่นเสียง
  duration = pulseIn(echoPin, HIGH);

  // คำนวณระยะทางจากเวลา (ความเร็วของเสียง 340 m/s)
  distanceCm = (duration * 0.0343) / 2;

  // แสดงระยะทางบน LCD
  lcd.setCursor(10, 0); // ตั้ง cursor ที่ตำแหน่งที่ 10 ของบรรทัดแรก
  lcd.print("       "); // ลบค่าก่อนหน้า
  lcd.setCursor(10, 0); // ตั้ง cursor ใหม่
  lcd.print(distanceCm); // แสดงระยะทางเป็นเซนติเมตร
  lcd.print(" cm");

  // แสดงข้อมูลบน Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distanceCm);
  Serial.println(" cm");

  // ควบคุม Servo ตามระยะทางที่วัดได้
  if (distanceCm < 50) {
    smoothMoveServo(180); // ถ้าระยะน้อยกว่า 50 เซนติเมตร ให้หมุนไปที่ 180 องศาอย่างนิ่มนวล
  } else {
    smoothMoveServo(0);   // ถ้าระยะมากกว่า 50 เซนติเมตร ให้หมุนกลับไปที่ 0 องศาอย่างนิ่มนวล
  }

  delay(500); // รอ 500 ms ก่อนการวัดครั้งต่อไป
}

// ฟังก์ชันสำหรับหมุน Servo อย่างนิ่มนวล
void smoothMoveServo(int targetAngle) {
  int currentAngle = myServo.read(); // อ่านมุมปัจจุบันของ Servo
  if (currentAngle < targetAngle) {
    for (int angle = currentAngle; angle <= targetAngle; angle++) {
      myServo.write(angle);
      delay(10); // ควบคุมความเร็วในการหมุน
    }
  } else {
    for (int angle = currentAngle; angle >= targetAngle; angle--) {
      myServo.write(angle);
      delay(10); // ควบคุมความเร็วในการหมุน
    }
  }
}