#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>

// --- ตั้งค่าจอ OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- ตั้งค่า Ultrasonic Sensor ---
#define TRIG_PIN  19
#define ECHO_PIN  18
#define MAX_DISTANCE 200 // ระยะไกลสุดที่จะวัด (cm)
#define DETECT_DIST  50  // *** ระยะที่ต้องการให้ไม้กั้นเปิด (เช่น 20 cm) ***

// --- ตั้งค่า Servo Motor ---
#define SERVO_PIN 17
Servo myServo;

void setup() {
  Serial.begin(115200);

  // 1. เริ่มต้นการทำงานของ Ultrasonic
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // 2. เริ่มต้นการทำงานของ Servo
  myServo.setPeriodHertz(50); // มาตรฐาน servo ทั่วไป 50Hz
  myServo.attach(SERVO_PIN, 500, 2400); // ค่า min/max pulse width สำหรับ ESP32
  myServo.write(0); // เริ่มต้นไม้กั้นปิด (0 องศา)

  // 3. เริ่มต้นการทำงานของจอ OLED
  // ที่อยู่ I2C ส่วนมากจะเป็น 0x3C ถ้าไม่ติดลองเปลี่ยนเป็น 0x3D
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // ถ้าจอไม่ติด ให้หยุดทำงานตรงนี้
  }
  
  // ตั้งค่า Font และสีตัวอักษร
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2); 
  
  // แสดงผลเริ่มต้น
  showDisplay("Initializing...");
  delay(1000);
}

void loop() {
  // --- ส่วนที่ 1: วัดระยะทาง ---
  long duration;
  int distance;

  // ส่งสัญญาณ Trig ออกไป 10 ไมโครวินาที
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // อ่านค่าสัญญาณสะท้อนกลับ (Echo)
  duration = pulseIn(ECHO_PIN, HIGH);
  
  // คำนวณเป็นเซนติเมตร
  distance = duration * 0.034 / 2;

  // ป้องกันค่า Error (ถ้าอ่านเป็น 0 หรือ ไกลมาก ให้ถือว่าไม่มีรถ)
  if (distance == 0 || distance > MAX_DISTANCE) {
    distance = MAX_DISTANCE;
  }

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // --- ส่วนที่ 2: ตรวจสอบเงื่อนไขและสั่งงาน ---
  
  if (distance <= DETECT_DIST) {
    // >> กรณีเจอวัตถุในระยะ (เปิดไม้กั้น) <<
    
    // สั่ง Servo หมุนไป 90 องศา
    myServo.write(90); 
    
    // แสดงข้อความ Welcome
    showDisplay("Welcome");
    
    // *เทคนิคเสริม* หน่วงเวลาเล็กน้อยเพื่อไม่ให้ Servo กระตุกเวลารถขยับเข้าออกที่เส้นขอบ
    delay(200); 

  } else {
    // >> กรณีไม่มีวัตถุ หรือ รถออกไปแล้ว (ปิดไม้กั้น) <<
    
    // สั่ง Servo กลับมาที่ 0 องศา
    myServo.write(0);
    
    // แสดงข้อความ Ready
    showDisplay("Ready");
  }
  
  delay(100); // หน่วงเวลาการวนลูปเล็กน้อยเพื่อความเสถียร
}

// ฟังก์ชันช่วยแสดงผลจอ OLED เพื่อลดความซ้ำซ้อนของโค้ด
void showDisplay(String message) {
  display.clearDisplay();
  display.setCursor(0, 20); // จัดตำแหน่งตัวอักษร (x, y)
  // จัดกึ่งกลางแบบง่ายๆ (ปรับตามความยาวคำ)
  if(message == "Welcome") display.setCursor(20, 25);
  if(message == "Ready") display.setCursor(35, 25);
  
  display.println(message);
  display.display();
}