#include <AccelStepper.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>
#include "esp_task_wdt.h"
#include "freertos/FreeRTOS.h"

// ================== PIN MAPPING ==================
#define PIN_STEP 26
#define PIN_DIR 27
#define PIN_ENABLE 33

// === LIMIT SWITCH PINTU ===
#define LIMIT_DOOR_OPEN 34
#define LIMIT_DOOR_CLOSE 35

// === FINAL LIMIT LIFT ===
#define FINAL_LIMIT_UP 32
#define FINAL_LIMIT_DOWN 25

// === TOMBOL MANUAL PINTU ===
#define BTN_OPEN 13
#define BTN_CLOSE 12

// === TOMBOL MANUAL MOTOR LIFT ===
#define BTN_DIR_UP 23
#define BTN_DIR_DOWN 14

// === EMERGENCY & MODE ===
#define BTN_EMERGENCY 19
#define SW_INSPECTION 18
#define SW_AUTO 5

// === UART (CTB MCC) ===
#define RXD2 16
#define TXD2 17

// === I2C OLED (PAKAI DEFAULT WIRE) ===
#define I2C_SDA 21
#define I2C_SCL 22

// === RELAY MOTOR LIFT ===
#define RELAY_MOTOR_UP 2
#define RELAY_MOTOR_DOWN 4

// ================== OBJECTS ==================
AccelStepper doorStepper(AccelStepper::DRIVER, PIN_STEP, PIN_DIR);
Adafruit_SSD1306 display(128, 64, &Wire);

// ================== VARIABLES ==================
enum DoorState {DOOR_STOP, DOOR_OPENING, DOOR_CLOSING, DOOR_ERROR};
DoorState doorState = DOOR_STOP;

enum CabinState {CABIN_STOP, CABIN_UP, CABIN_DOWN};
CabinState cabinState = CABIN_STOP;

// Auto-close variables
unsigned long doorOpenTime = 0;
bool autoCloseActive = false;
const unsigned long autoCloseDelay = 3000;

// Homing variables
long steps_per_door = 0;
bool is_calibrated = false;
const int EEPROM_ADDRESS = 0;
const unsigned long HOMING_TIMEOUT = 3000;

// Debounce variables
const unsigned long DEBOUNCE_DELAY = 50;
unsigned long lastDebounceTimeBtnOpen = 0;
int lastBtnOpenState = HIGH;
unsigned long lastDebounceTimeBtnClose = 0;
int lastBtnCloseState = HIGH;
unsigned long lastDebounceTimeBtnDirUp = 0;
int lastBtnDirUpState = HIGH;
unsigned long lastDebounceTimeBtnDirDown = 0;
int lastBtnDirDownState = HIGH;
unsigned long lastDebounceTimeBtnEmergency = 0;
int lastBtnEmergencyState = HIGH;
unsigned long lastDebounceTimeSwInspection = 0;
int lastSwInspectionState = HIGH;
unsigned long lastDebounceTimeSwAuto = 0;
int lastSwAutoState = HIGH;
unsigned long lastDebounceTimeLimitDoorOpen = 0;
int lastLimitDoorOpenState = HIGH;
unsigned long lastDebounceTimeLimitDoorClose = 0;
int lastLimitDoorCloseState = HIGH;
unsigned long lastDebounceTimeFinalLimitUp = 0;
int lastFinalLimitUpState = HIGH;
unsigned long lastDebounceTimeFinalLimitDown = 0;
int lastFinalLimitDownState = HIGH;

// Display refresh timer
unsigned long lastDisplayUpdateTime = 0;
const unsigned long displayUpdateInterval = 80;

// Door stepper params
const float MAX_SPEED = 900.0f;
const float MIN_SPEED = 200.0f;
const float ACCEL = 600.0f;
const long SLOWDOWN_ZONE = 200L;

// UART command variables for AUTO mode
String doorCommand = "";
String cabinCommand = "";
unsigned long lastSerialReadTime = 0;
const unsigned long SERIAL_TIMEOUT = 1000; // Timeout 1 detik

// Error handling variables (non-blocking)
bool inErrorState = false;
unsigned long errorStartTime = 0;
const unsigned long ERROR_DISPLAY_DURATION = 5000; // 5 detik

// ================== FUNCTIONS ==================

int debouncedDigitalRead(int pin, unsigned long* lastDebounceTime, int* lastState) {
    int reading = digitalRead(pin);
    if (reading != *lastState) {
        *lastDebounceTime = millis();
    }
    *lastState = reading;
    if ((millis() - *lastDebounceTime) > DEBOUNCE_DELAY) {
        return reading;
    }
    return *lastState;
}

void stopCabinMotor() {
    digitalWrite(RELAY_MOTOR_UP, LOW);
    digitalWrite(RELAY_MOTOR_DOWN, LOW);
    cabinState = CABIN_STOP;
}

void displayError(const char* message) {
    if (!inErrorState) {
        Serial.println(message);
        display.clearDisplay();
        display.setTextSize(1);
        display.setTextColor(SSD1306_WHITE);
        display.setCursor(0, 0);
        display.println("--- SYSTEM ERROR ---");
        display.setCursor(0, 15);
        display.println(message);
        display.display();
        inErrorState = true;
        errorStartTime = millis();
    }

    // Cek durasi error display, jika sudah selesai, restart sistem
    if (millis() - errorStartTime >= ERROR_DISPLAY_DURATION) {
        ESP.restart();
    }
}

void handleAutoMode() {
    if (Serial2.available()) {
        String command = Serial2.readStringUntil('\n');
        command.trim();
        lastSerialReadTime = millis();

        // Perintah untuk motor kabin (hanya jalan jika pintu STOP dan tertutup penuh)
        if (doorState == DOOR_STOP && debouncedDigitalRead(LIMIT_DOOR_CLOSE, &lastDebounceTimeLimitDoorClose, &lastLimitDoorCloseState) == LOW) {
            if (command == "UP" && debouncedDigitalRead(FINAL_LIMIT_UP, &lastDebounceTimeFinalLimitUp, &lastFinalLimitUpState) == HIGH) {
                cabinCommand = "UP";
            } 
            else if (command == "DOWN" && debouncedDigitalRead(FINAL_LIMIT_DOWN, &lastDebounceTimeFinalLimitDown, &lastFinalLimitDownState) == HIGH) {
                cabinCommand = "DOWN";
            }
            else if (command == "STOP_CABIN") {
                cabinCommand = "STOP";
            }
        }

        // Perintah untuk pintu (hanya jalan jika kabin STOP)
        if (cabinState == CABIN_STOP) {
            if (command == "OPEN") {
                doorCommand = "OPEN";
            }
            else if (command == "CLOSE") {
                doorCommand = "CLOSE";
            }
        }
    }

    // Timeout UART: Reset perintah jika tidak ada data baru selama 1 detik
    if (millis() - lastSerialReadTime > SERIAL_TIMEOUT) {
        doorCommand = "";
        cabinCommand = "";
    }

    // Eksekusi perintah kabin
    if (cabinCommand == "UP") {
        digitalWrite(RELAY_MOTOR_UP, HIGH);
        digitalWrite(RELAY_MOTOR_DOWN, LOW);
        cabinState = CABIN_UP;
    }
    else if (cabinCommand == "DOWN") {
        digitalWrite(RELAY_MOTOR_DOWN, HIGH);
        digitalWrite(RELAY_MOTOR_UP, LOW);
        cabinState = CABIN_DOWN;
    }
    else if (cabinCommand == "STOP") {
        stopCabinMotor();
        cabinCommand = "";
    }
}

void homingAndCalibration() {
    Serial.println("Memulai proses homing dan kalibrasi...");
    EEPROM.get(EEPROM_ADDRESS, steps_per_door);
    
    // Cek apakah nilai kalibrasi sudah ada di EEPROM
    if (steps_per_door > 0) {
        Serial.println("Nilai kalibrasi ditemukan di EEPROM.");
        is_calibrated = true;
        doorStepper.setCurrentPosition(0);
        Serial.print("Steps per door: ");
        Serial.println(steps_per_door);
        return;
    }

    Serial.println("Nilai kalibrasi tidak ditemukan. Melakukan kalibrasi baru.");

    // Langkah 1: Homing, mencari posisi pintu tertutup (posisi 0)
    Serial.println("Mencari posisi 0 (pintu tertutup)...");
    doorStepper.setMaxSpeed(MIN_SPEED);
    doorStepper.setAcceleration(ACCEL);
    doorStepper.moveTo(-20000); // Gerakkan motor ke arah negatif hingga limit switch tertutup
    unsigned long homing_start_time = millis();
    while(debouncedDigitalRead(LIMIT_DOOR_CLOSE, &lastDebounceTimeLimitDoorClose, &lastLimitDoorCloseState) == HIGH) {
        doorStepper.run();
        if (millis() - homing_start_time > HOMING_TIMEOUT) {
            displayError("Homing Error: Timeout limit tutup");
            return;
        }
        esp_task_wdt_reset();
    }
    doorStepper.stop();
    doorStepper.setCurrentPosition(0);
    Serial.println("Posisi 0 ditemukan.");

    // Langkah 2: Mengukur langkah dari pintu tutup ke pintu buka
    Serial.println("Mengukur langkah dari pintu tutup ke pintu buka...");
    doorStepper.setMaxSpeed(MAX_SPEED);
    doorStepper.setAcceleration(ACCEL);
    doorStepper.moveTo(20000); // Gerakkan motor ke arah positif hingga limit switch terbuka
    homing_start_time = millis();
    while(debouncedDigitalRead(LIMIT_DOOR_OPEN, &lastDebounceTimeLimitDoorOpen, &lastLimitDoorOpenState) == HIGH) {
        doorStepper.run();
        if (millis() - homing_start_time > HOMING_TIMEOUT) {
            displayError("Homing Error: Timeout limit buka");
            return;
        }
        esp_task_wdt_reset();
    }
    doorStepper.stop();
    long end_position = doorStepper.currentPosition();
    doorStepper.setCurrentPosition(end_position);
    steps_per_door = end_position;
    EEPROM.put(EEPROM_ADDRESS, steps_per_door);
    is_calibrated = true;
    Serial.print("Kalibrasi selesai. Steps per door: ");
    Serial.println(steps_per_door);
}

void printStatusToSerial() {
    bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);
    const char* modeStr = (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) ? "INSPECTION" : "AUTO";
    const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
    const char* doorStr = (doorState == DOOR_OPENING) ? "OPENING" : (doorState == DOOR_CLOSING) ? "CLOSING" : "IDLE";
    int remaining = 0;
    if ((doorState == DOOR_OPENING || doorState == DOOR_STOP) && autoCloseActive) {
        unsigned long elapsed = millis() - doorOpenTime;
        if (elapsed < autoCloseDelay) {
            remaining = (autoCloseDelay - elapsed) / 1000 + 1;
        }
    }

    Serial.print("Mode: ");
    Serial.print(modeStr);
    Serial.print(" | Dir: ");
    Serial.print(cabinStr);
    Serial.print(" | Door: ");
    Serial.print(doorStr);
    if (remaining > 0) {
        Serial.print(" | AutoClose in ");
        Serial.print(remaining);
        Serial.print("s");
    }
    Serial.print(" | EMG: ");
    Serial.println(emergency ? "ON" : "OFF");

    Serial.print(" | L_UP: ");
    Serial.print((digitalRead(FINAL_LIMIT_UP) == LOW) ? "ON" : "OFF");
    Serial.print(" | L_DN: ");
    Serial.println((digitalRead(FINAL_LIMIT_DOWN) == LOW) ? "ON" : "OFF");
}

void displayModeInfo(const char* mode, const char* cabinDir, const char* doorStateStr, bool emergency) {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(3, 0);
    display.println("Orch_CTBDD");
    display.drawLine(0, 18, 128, 18, SSD1306_WHITE);
    display.setTextSize(1);
    display.setCursor(0, 22);
    display.print("MODE");
    display.setCursor(25, 22);
    display.print(":");
    display.setCursor(31, 22);
    display.println(mode);
    display.setCursor(0, 33);
    display.print("DIR");
    display.setCursor(25, 33);
    display.print(":");
    display.setCursor(31, 33);
    display.println(cabinDir);
    display.setCursor(0, 44);
    display.print("DOOR");
    display.setCursor(25, 44);
    display.print(":");
    display.setCursor(31, 44);
    display.print(doorStateStr);

    if ((doorState == DOOR_OPENING || doorState == DOOR_STOP) && autoCloseActive) {
        unsigned long elapsed = millis() - doorOpenTime;
        if (elapsed < autoCloseDelay) {
            int remaining = (autoCloseDelay - elapsed) / 1000 + 1;
            display.setCursor(55, 44);
            display.print(",Close in ");
            display.print(remaining);
            display.print("s");
        }
    }

    display.setCursor(0, 56);
    display.print("F.LM");
    display.setCursor(25, 56);
    display.print(":");
    const char* finalLimitStatus;
    if (digitalRead(FINAL_LIMIT_UP) == LOW) {
        finalLimitStatus = "UP ACTIVE";
    } else if (digitalRead(FINAL_LIMIT_DOWN) == LOW) {
        finalLimitStatus = "DN ACTIVE";
    } else {
        finalLimitStatus = "OFF";
    }
    display.print(finalLimitStatus);

    if (emergency) {
        display.fillRect(0, 22, 128, 40, SSD1306_WHITE);
        display.setTextColor(SSD1306_BLACK);
        display.setCursor(8, 28);
        display.setTextSize(1);
        display.println("*** DARURAT! ***");
        display.setCursor(8, 44);
        display.println("Lift dihentikan.");
        display.setTextColor(SSD1306_WHITE);
    } else {
        display.setTextSize(1);
        display.setCursor(72, 33);
        display.print("EM.S:OFF");
    }
    display.display();
}

void updateDisplay() {
    if (millis() - lastDisplayUpdateTime >= displayUpdateInterval) {
        lastDisplayUpdateTime = millis();
        bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);
        const char* modeStr = (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) ? "INSPECTION" : "AUTO";
        const char* cabinStr = (cabinState == CABIN_UP) ? "UP" : (cabinState == CABIN_DOWN) ? "DOWN" : "IDLE";
        const char* doorStr = (doorState == DOOR_OPENING) ? "OPENING" : (doorState == DOOR_CLOSING) ? "CLOSING" : "IDLE";
        
        if (doorState == DOOR_ERROR) {
            displayError("Kalibrasi Gagal!");
            return;
        }

        if (inErrorState) {
            displayError(""); // Terus tampilkan pesan error
            return;
        }

        displayModeInfo(modeStr, cabinStr, doorStr, emergency);
        printStatusToSerial();
    }
}

void showStartup() {
    display.clearDisplay();
    display.setTextSize(3);
    display.setCursor(132, 0);
    display.setTextColor(SSD1306_WHITE);
    display.print("Orch.Gen");
    display.display();
    for (int x = 132; x > 20; x -= 4) {
        display.clearDisplay();
        display.setTextSize(2);
        display.setCursor(x, 0);
        display.print("Orch.Gen");
        display.display();
        delay(15);
    }
    display.setTextSize(1);
    display.setCursor(38, 25);
    display.print("By Elzabid");
    display.display();
    delay(300);
    for (int i = 0; i < 3; i++) {
        display.setTextSize(1);
        display.setCursor(20, 45);
        display.print("Initializing...");
        display.display();
        delay(300);
        display.clearDisplay();
        display.setCursor(20, 45);
        display.display();
        delay(300);
    }
    display.setTextSize(1);
    display.setCursor(0, 55);
    display.print("Please wait...");
    for (int i = 0; i < 120; i += 6) {
        display.fillRect(0, 50, i, 3, SSD1306_WHITE);
        display.display();
        delay(20);
    }
    Serial.println("Startup selesai. Sistem siap.");
}

// ================== SETUP ==================
void setup() {
    Serial.begin(115200);
    Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);
    Serial.println("Sistem dinyalakan.");

    esp_task_wdt_config_t twdt_config = {
        .timeout_ms = 1000,
        .idle_core_mask = (1 << xPortGetCoreID()),
        .trigger_panic = true,
    };
    esp_task_wdt_init(&twdt_config);
    esp_task_wdt_add(NULL);

    pinMode(LIMIT_DOOR_OPEN, INPUT_PULLUP);
    pinMode(LIMIT_DOOR_CLOSE, INPUT_PULLUP);
    pinMode(FINAL_LIMIT_UP, INPUT_PULLUP);
    pinMode(FINAL_LIMIT_DOWN, INPUT_PULLUP);
    pinMode(BTN_OPEN, INPUT_PULLUP);
    pinMode(BTN_CLOSE, INPUT_PULLUP);
    pinMode(BTN_DIR_UP, INPUT_PULLUP);
    pinMode(BTN_DIR_DOWN, INPUT_PULLUP);
    pinMode(BTN_EMERGENCY, INPUT_PULLUP);
    pinMode(SW_INSPECTION, INPUT_PULLUP);
    pinMode(SW_AUTO, INPUT_PULLUP);
    pinMode(RELAY_MOTOR_UP, OUTPUT);
    pinMode(RELAY_MOTOR_DOWN, OUTPUT);

    stopCabinMotor();

    pinMode(PIN_ENABLE, OUTPUT);
    digitalWrite(PIN_ENABLE, LOW);

    doorStepper.setMaxSpeed(MAX_SPEED);
    doorStepper.setAcceleration(ACCEL);

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("Alokasi SSD1306 gagal"));
        for (;;);
    }
    
    showStartup();
    homingAndCalibration();

    updateDisplay();
}

// ================== LOOP ==================
void loop() {
    esp_task_wdt_reset();

    // Jika sistem dalam keadaan error, hanya tampilkan error dan tunggu restart
    if (inErrorState) {
        updateDisplay();
        return;
    }

    // Cek tombol darurat setiap saat
    bool emergency = (debouncedDigitalRead(BTN_EMERGENCY, &lastDebounceTimeBtnEmergency, &lastBtnEmergencyState) == LOW);
    if (emergency) {
        stopCabinMotor();
        doorStepper.stop();
        doorState = DOOR_STOP;
        updateDisplay();
        return;
    }

    // Jika kalibrasi gagal, masuk ke state error
    if (!is_calibrated) {
        doorStepper.stop();
        doorState = DOOR_ERROR;
        updateDisplay();
        return;
    }
    
    // Logika utama hanya berjalan jika tidak ada kondisi error/darurat
    
    // Mode INSPECTION
    if (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) == LOW) {
        if (doorState == DOOR_STOP) { 
            // Gerakkan kabin secara manual hanya jika pintu dalam keadaan STOP
            if (debouncedDigitalRead(BTN_DIR_UP, &lastDebounceTimeBtnDirUp, &lastBtnDirUpState) == LOW && debouncedDigitalRead(FINAL_LIMIT_UP, &lastDebounceTimeFinalLimitUp, &lastFinalLimitUpState) == HIGH) {
                digitalWrite(RELAY_MOTOR_UP, HIGH);
                digitalWrite(RELAY_MOTOR_DOWN, LOW);
                cabinState = CABIN_UP;
            }
            else if (debouncedDigitalRead(BTN_DIR_DOWN, &lastDebounceTimeBtnDirDown, &lastBtnDirDownState) == LOW && debouncedDigitalRead(FINAL_LIMIT_DOWN, &lastDebounceTimeFinalLimitDown, &lastFinalLimitDownState) == HIGH) {
                digitalWrite(RELAY_MOTOR_DOWN, HIGH);
                digitalWrite(RELAY_MOTOR_UP, LOW);
                cabinState = CABIN_DOWN;
            }
            else {
                stopCabinMotor();
            }
        } else {
            // Hentikan motor kabin jika pintu sedang bergerak
            stopCabinMotor();
        }
    } 
    // Logika untuk mode AUTO
    else { 
        handleAutoMode();
    }

    // DOOR OPERATOR
    switch (doorState) {
        case DOOR_STOP:
            if ((debouncedDigitalRead(BTN_OPEN, &lastDebounceTimeBtnOpen, &lastBtnOpenState) == LOW || doorCommand == "OPEN") && debouncedDigitalRead(LIMIT_DOOR_CLOSE, &lastDebounceTimeLimitDoorClose, &lastLimitDoorCloseState) == LOW) {
                doorState = DOOR_OPENING;
                doorStepper.moveTo(steps_per_door);
                autoCloseActive = false;
                doorCommand = "";
            }
            else if ((debouncedDigitalRead(BTN_CLOSE, &lastDebounceTimeBtnClose, &lastBtnCloseState) == LOW || doorCommand == "CLOSE") && debouncedDigitalRead(LIMIT_DOOR_OPEN, &lastDebounceTimeLimitDoorOpen, &lastLimitDoorOpenState) == LOW) {
                doorState = DOOR_CLOSING;
                doorStepper.moveTo(0);
                doorCommand = "";
            }
            break;

        case DOOR_OPENING:
            if (doorStepper.distanceToGo() == 0) {
                doorState = DOOR_STOP;
                doorStepper.setCurrentPosition(steps_per_door);
                // Aktifkan auto-close hanya di mode AUTO
                if (debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) != LOW) {
                    doorOpenTime = millis();
                    autoCloseActive = true;
                }
            }
            else if (debouncedDigitalRead(BTN_CLOSE, &lastDebounceTimeBtnClose, &lastBtnCloseState) == LOW || doorCommand == "CLOSE") {
                doorState = DOOR_CLOSING;
                doorStepper.moveTo(0);
                autoCloseActive = false;
                doorCommand = "";
            }
            break;

        case DOOR_CLOSING:
            if (doorStepper.distanceToGo() == 0) {
                doorState = DOOR_STOP;
                doorStepper.setCurrentPosition(0);
            }
            else if (debouncedDigitalRead(BTN_OPEN, &lastDebounceTimeBtnOpen, &lastBtnOpenState) == LOW || doorCommand == "OPEN") {
                doorState = DOOR_OPENING;
                doorStepper.moveTo(steps_per_door);
                doorCommand = "";
            }
            break;
    }

    // Auto-close process
    if (autoCloseActive && debouncedDigitalRead(SW_INSPECTION, &lastDebounceTimeSwInspection, &lastSwInspectionState) != LOW) {
        if (millis() - doorOpenTime >= autoCloseDelay) {
            doorState = DOOR_CLOSING;
            doorStepper.moveTo(0);
            autoCloseActive = false;
        }
    }

    doorStepper.run();
    updateDisplay();
}