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

// กำหนดขา Ultrasonic
#define TRIG_PIN 18 // ขาสำหรับ Trig
#define ECHO_PIN 19 // ขาสำหรับ Echo

// กำหนดขา LED
#define RED_LED_PIN 4   // ขาสำหรับไฟ LED สีแดง
#define GREEN_LED_PIN 5 // ขาสำหรับไฟ LED สีเขียว

// กำหนดระยะเกณฑ์ (หน่วย: เซนติเมตร)
const float DISTANCE_THRESHOLD = 200.0;

// กำหนดขนาดจอ OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// สร้างอ็อบเจ็กต์จอ OLED
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {
  // ตั้งค่าขา GPIO
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);

  // เริ่มต้น Serial Monitor
  Serial.begin(115200);

  // เริ่มต้นจอ OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // ใช้คำสั่งที่ถูกต้อง
    Serial.println("OLED initialization failed!");
    while (true); // หยุดการทำงานหากจอ OLED ไม่พร้อม
  }

  // ล้างหน้าจอ OLED และแสดงข้อความเริ่มต้น
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();
  delay(2000);

  // ปิด LED เริ่มต้น
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(GREEN_LED_PIN, LOW);
}

void loop() {
  // วัดระยะทางด้วย Ultrasonic
  float distance = measureDistance();

  // แสดงผลใน Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // แสดงผลบนจอ OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.println("Ultrasonic Sensor");
  display.setTextSize(2);
  display.setCursor(0, 20);
  display.print("Dist: ");
  display.print(distance);
  display.println(" cm");

  // ตรวจสอบระยะทางและควบคุม LED
  if (distance > DISTANCE_THRESHOLD) {
    digitalWrite(RED_LED_PIN, HIGH);   // เปิดไฟ LED สีแดง
    digitalWrite(GREEN_LED_PIN, LOW); // ปิดไฟ LED สีเขียว
    display.setTextSize(1);
    display.setCursor(0, 50);
    display.println("Status: Too Far!");
  } else {
    digitalWrite(RED_LED_PIN, LOW);    // ปิดไฟ LED สีแดง
    digitalWrite(GREEN_LED_PIN, HIGH); // เปิดไฟ LED สีเขียว
    display.setTextSize(1);
    display.setCursor(0, 50);
    display.println("Status: In Range");
  }

  // อัปเดตจอ OLED
  display.display();

  // หน่วงเวลา 500 มิลลิวินาที
  delay(500);
}

// ฟังก์ชันวัดระยะทางด้วย Ultrasonic
float measureDistance() {
  // ส่งสัญญาณ Trig
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // อ่านเวลาจาก Echo
  long duration = pulseIn(ECHO_PIN, HIGH);

  // คำนวณระยะทาง (หน่วย: เซนติเมตร)
  float distance = duration * 0.034 / 2.0;

  return distance;
}