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

// ==== PIN SETUP ====
#define TRIG_PIN 5
#define ECHO_PIN 18

// ==== LCD ====
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ==== TASKS DECLARES ====
void readUltrasonic();
void updateLCD();

// Task: 50ms đọc khoảng cách
Task tReadUS(50, TASK_FOREVER, &readUltrasonic);
// Task: 200ms cập nhật LCD
Task tUpdateLCD(200, TASK_FOREVER, &updateLCD);

// ==== Scheduler ====
Scheduler runner;

float distanceCM = 0;

// ======================================================
//      HÀM ĐỌC HC-SR04 (KHÔNG DÙNG DELAY BLOCK)
// ======================================================
void readUltrasonic() {
    // Reset pulse
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);

    // Tạo xung 10us
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // Đo xung echo
    long duration = pulseIn(ECHO_PIN, HIGH, 30000); // timeout 30ms tránh treo

    // Tính khoảng cách
    distanceCM = duration * 0.0343 / 2;
}

// ======================================================
//                HIỂN THỊ LÊN LCD
// ======================================================
void updateLCD() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Distance:");

    lcd.setCursor(0, 1);
    lcd.print(distanceCM, 1);
    lcd.print(" cm");
}

// ======================================================
//                        SETUP
// ======================================================
void setup() {
    Serial.begin(115200);

    // Pin mode
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);

    // LCD
    lcd.init();
    lcd.backlight();

    // Add tasks
    runner.addTask(tReadUS);
    runner.addTask(tUpdateLCD);

    // Enable tasks
    tReadUS.enable();
    tUpdateLCD.enable();
}

// ======================================================
//                        LOOP
// ======================================================
void loop() {
    runner.execute();
}