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

// Definisi ukuran layar OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Membuat objek OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Definisi pin tombol
#define UP_BUTTON_PIN 12
#define DOWN_BUTTON_PIN 13
#define SELECT_BUTTON_PIN 14

// Definisi pin untuk sensor ultrasonik (Trig dan Echo untuk 6 sensor)
#define TRIG_PIN_1 2
#define ECHO_PIN_1 4
#define TRIG_PIN_2 5
#define ECHO_PIN_2 18
#define TRIG_PIN_3 19
#define ECHO_PIN_3 23
#define TRIG_PIN_4 17
#define ECHO_PIN_4 16
#define TRIG_PIN_5 25
#define ECHO_PIN_5 26
#define TRIG_PIN_6 27
#define ECHO_PIN_6 32

// Variabel untuk mengelola menu
int menuIndex = 0;
int submenuIndex = 0;
bool isSubmenu = false;
bool verificationStep = false;

// Batas deteksi (dalam cm) untuk menentukan slot penuh
const int detectionThreshold = 27;

// Definisi struktur menu dan sub-menu
const int mainMenuItems = 3;
String mainMenu[mainMenuItems] = {"Lantai 1", "Lantai 2", "Lantai 3"};
String subMenu[mainMenuItems][2] = {
    {"Slot 1", "Slot 2"},
    {"Slot 1", "Slot 2"},
    {"Slot 1", "Slot 2"}
};

// Variabel untuk menyimpan status tampilan terakhir
int lastMenuIndex = -1;
int lastSubmenuIndex = -1;
bool lastIsSubmenu = false;
bool lastVerificationStep = false;

void setup() {
    // Inisialisasi pin tombol
    pinMode(UP_BUTTON_PIN, INPUT_PULLUP);
    pinMode(DOWN_BUTTON_PIN, INPUT_PULLUP);
    pinMode(SELECT_BUTTON_PIN, INPUT_PULLUP);

    // Inisialisasi pin sensor ultrasonik
    pinMode(TRIG_PIN_1, OUTPUT);
    pinMode(ECHO_PIN_1, INPUT);
    pinMode(TRIG_PIN_2, OUTPUT);
    pinMode(ECHO_PIN_2, INPUT);
    pinMode(TRIG_PIN_3, OUTPUT);
    pinMode(ECHO_PIN_3, INPUT);
    pinMode(TRIG_PIN_4, OUTPUT);
    pinMode(ECHO_PIN_4, INPUT);
    pinMode(TRIG_PIN_5, OUTPUT);
    pinMode(ECHO_PIN_5, INPUT);
    pinMode(TRIG_PIN_6, OUTPUT);
    pinMode(ECHO_PIN_6, INPUT);

    // Inisialisasi OLED display
    if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        for(;;);
    }
    display.display();
    delay(1000);
    display.clearDisplay();
}

void loop() {
    if (!verificationStep) {
        if (digitalRead(UP_BUTTON_PIN) == LOW) {
            if (isSubmenu) {
                submenuIndex = (submenuIndex == 0) ? 1 : 0;
            } else {
                menuIndex = (menuIndex == 0) ? mainMenuItems - 1 : menuIndex - 1;
            }
            delay(200);
        }
        if (digitalRead(DOWN_BUTTON_PIN) == LOW) {
            if (isSubmenu) {
                submenuIndex = (submenuIndex == 1) ? 0 : 1;
            } else {
                menuIndex = (menuIndex >= mainMenuItems - 1) ? 0 : menuIndex + 1;
            }
            delay(200);
        }
        if (digitalRead(SELECT_BUTTON_PIN) == LOW) {
            verificationStep = true;  // Aktifkan verifikasi dua langkah
            delay(200);
        }
    } else {
        displayVerification();
        if (digitalRead(UP_BUTTON_PIN) == LOW) {
            isSubmenu = !isSubmenu;
            verificationStep = false;
            delay(200);
        }
        if (digitalRead(DOWN_BUTTON_PIN) == LOW) {
            verificationStep = false;
            delay(200);
        }
    }

    if (menuIndex != lastMenuIndex || submenuIndex != lastSubmenuIndex || isSubmenu != lastIsSubmenu || verificationStep != lastVerificationStep) {
        displayMenu();
        lastMenuIndex = menuIndex;
        lastSubmenuIndex = submenuIndex;
        lastIsSubmenu = isSubmenu;
        lastVerificationStep = verificationStep;
    }
}

void displayMenu() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);

    if (verificationStep) {
        displayVerification();
    } else if (isSubmenu) {
        display.setCursor(0, 0);
        display.println("Status Slot:");

        for (int i = 0; i < 2; i++) {
            int distance = getDistance(menuIndex * 2 + i + 1);
            String status = (distance < detectionThreshold) ? "Penuh" : "Kosong";

            display.setTextColor(i == submenuIndex ? SSD1306_BLACK : SSD1306_WHITE, SSD1306_WHITE);
            display.setCursor(10, i * 10 + 10);
            display.print(subMenu[menuIndex][i]);
            display.print(": ");
            display.println(status);
        }
    } else {
        display.setCursor(0, 0);
        display.println("Pilih Lantai:");

        for (int i = 0; i < mainMenuItems; i++) {
            display.setTextColor(i == menuIndex ? SSD1306_BLACK : SSD1306_WHITE, SSD1306_WHITE);
            display.setCursor(10, i * 10 + 10);
            display.println(mainMenu[i]);

            // Tambahkan pesan jika semua slot di lantai ini penuh
            if (isFloorFull(i)) {
                display.setCursor(10, i * 10 + 20); // Geser sedikit ke bawah untuk pesan
                display.println("Slot penuh");
            }
        }
    }
    display.display();
}

void displayVerification() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 10);
    display.println("Masuk ke menu ini?");
    display.setCursor(0, 20);
    display.println("Iya: Tekan UP");
    display.setCursor(0, 30);
    display.println("Tidak: Tekan DOWN");
    display.display();
}

int getDistance(int sensorID) {
    int trigPin, echoPin;
    switch(sensorID) {
        case 1: trigPin = TRIG_PIN_1; echoPin = ECHO_PIN_1; break;
        case 2: trigPin = TRIG_PIN_2; echoPin = ECHO_PIN_2; break;
        case 3: trigPin = TRIG_PIN_3; echoPin = ECHO_PIN_3; break;
        case 4: trigPin = TRIG_PIN_4; echoPin = ECHO_PIN_4; break;
        case 5: trigPin = TRIG_PIN_5; echoPin = ECHO_PIN_5; break;
        case 6: trigPin = TRIG_PIN_6; echoPin = ECHO_PIN_6; break;
        default: return -1;
    }

    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH);
    int distance = duration * 0.034 / 2;

    return distance;
}

bool isFloorFull(int floor) {
    for (int i = 0; i < 2; i++) {
        int sensorID = floor * 2 + i + 1;
        int distance = getDistance(sensorID);
        if (distance >= detectionThreshold) {
            return false; // Ada slot kosong
        }
    }
    return true; // Semua slot penuh
}