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

#define TRIG_PIN 9
#define ECHO_PIN 8

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDR 0x3C

Adafruit_SSD1306 display(
  SCREEN_WIDTH, SCREEN_HEIGHT,
  &Wire, OLED_RESET
);

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

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  digitalWrite(TRIG_PIN, LOW);

  Wire.begin();

  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println("OLED initialization failed!");
    while (true);
  }

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(5, 20);
  display.println("ULTRASONIC SENSOR");
  display.setCursor(20, 38);
  display.println("Initializing...");
  display.display();

  delay(1000);
}

void loop() {
  // ส่ง Trigger ความยาว 10 microseconds
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // อ่าน Echo พร้อม Timeout 30 ms
  unsigned long duration =
    pulseIn(ECHO_PIN, HIGH, 30000UL);

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  display.setTextSize(1);
  display.setCursor(12, 0);
  display.println("DISTANCE METER");
  display.drawLine(0, 12, 127, 12, SSD1306_WHITE);

  if (duration == 0) {
    display.setTextSize(1);
    display.setCursor(15, 30);
    display.println("OUT OF RANGE");

    Serial.println("No Echo");
  } else {
    // ความเร็วเสียงประมาณ 0.0343 cm/us
    float distance = duration * 0.0343 / 2.0;

    display.setTextSize(2);
    display.setCursor(4, 24);
    display.print(distance, 1);
    display.println(" cm");

    display.setTextSize(1);
    display.setCursor(0, 54);
    display.println("HC-SR04 ACTIVE");

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

  display.display();
  delay(200);
}