#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>

// Konfigurasi LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2); // Alamat I2C LCD, 16 kolom, 2 baris

// Konfigurasi Servo
Servo servo1;
Servo servo2;

// Pin tombol
const int buttonUp = 12;
const int buttonDown = 13;
const int buttonOk = 14;

// Variabel global
volatile int selectedIndex = 0;             // Huruf yang dipilih
const char letters[] = "ABCDEFGHIJKLM";     // Array huruf
volatile bool okPressed = false;            // Status tombol OK

// Variabel debounce tombol
unsigned long lastDebounceTime = 0;         // Waktu terakhir tombol ditekan
const unsigned long debounceDelay = 200;    // Delay debounce (ms)

// Fungsi membaca tombol dengan debounce
bool isButtonPressed(int pin) {
    if (digitalRead(pin) == LOW) {
        unsigned long currentTime = millis();
        if (currentTime - lastDebounceTime > debounceDelay) {
            lastDebounceTime = currentTime;
            return true;
        }
    }
    return false;
}

// Fungsi membaca tombol Up
void checkButtonUp() {
    if (isButtonPressed(buttonUp)) {
        selectedIndex = (selectedIndex + 1) % strlen(letters); // Pindah ke huruf berikutnya
    }
}

// Fungsi membaca tombol Down
void checkButtonDown() {
    if (isButtonPressed(buttonDown)) {
        selectedIndex = (selectedIndex - 1 + strlen(letters)) % strlen(letters); // Pindah ke huruf sebelumnya
    }
}

// Fungsi membaca tombol OK
void checkButtonOk() {
    if (isButtonPressed(buttonOk)) {
        okPressed = true; // Tandai tombol OK ditekan
    }
}

// Task untuk GUI LCD
void TaskGUI(void *pvParameters) {
    while (true) {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Pilih Huruf:");
        lcd.setCursor(0, 1);
        lcd.print(letters[selectedIndex]);
        vTaskDelay(200 / portTICK_PERIOD_MS); // Update setiap 200 ms
    }
}

// Task untuk kontrol Servo
void TaskActuator(void *pvParameters) {
    while (true) {
        if (okPressed) {
            okPressed = false; // Reset status tombol OK
            int position = map(selectedIndex, 0, strlen(letters) - 1, 0, 180); // Map huruf ke sudut servo
            servo1.write(position);
            servo2.write(position);
            vTaskDelay(1000 / portTICK_PERIOD_MS); // Tunggu servo bergerak
            servo1.write(0);
            servo2.write(0);
        }
        vTaskDelay(100 / portTICK_PERIOD_MS); // Periksa status setiap 100 ms
    }
}

// Task untuk input tombol
void TaskInput(void *pvParameters) {
    while (true) {
        checkButtonUp();    // Periksa tombol Up
        checkButtonDown();  // Periksa tombol Down
        checkButtonOk();    // Periksa tombol OK
        vTaskDelay(50 / portTICK_PERIOD_MS); // Cek tombol setiap 50 ms
    }
}

void setup() {
    Serial.begin(115200);
    Wire.begin(); // Inisialisasi I2C

    // Inisialisasi LCD
    lcd.begin(16, 2);
    lcd.init();
    lcd.backlight();

    // Inisialisasi Servo
    servo1.attach(5);
    servo2.attach(18);

    // Inisialisasi tombol
    pinMode(buttonUp, INPUT_PULLUP);
    pinMode(buttonDown, INPUT_PULLUP);
    pinMode(buttonOk, INPUT_PULLUP);

    // Membuat tugas FreeRTOS
    xTaskCreate(TaskGUI, "TaskGUI", 4096, NULL, 1, NULL);
    xTaskCreate(TaskInput, "TaskInput", 2048, NULL, 1, NULL);
    xTaskCreate(TaskActuator, "TaskActuator", 2048, NULL, 1, NULL);
}

void loop() {
    // Tidak digunakan
}