// ==========================================================
// === MCC_ORCH_SS - ADAPTIVE (v1)                         ===
// ==========================================================
// Target: Elevator Main Controller (Orchestrator)
// Deskripsi: Sistem kontrol lift non-blocking dengan logger
// ==========================================================

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <cstdio>
#include <cstdarg>

// ==========================================================
// 1. NON-BLOCKING LOGGER (DEBUG-FRIENDLY)
// ==========================================================
#define LOG_LEVEL           2
#define LOG_BUFFER_SIZE     15
#define LOG_MESSAGE_MAX_LEN 128

// Timestamp humanized hanya di DEBUG
#define LOG_HUMANIZED 1

struct LogMessage {
    char message[LOG_MESSAGE_MAX_LEN];
};

LogMessage logBuffer[LOG_BUFFER_SIZE];
volatile int logHead  = 0;
volatile int logTail  = 0;
volatile int logCount = 0;

void humanizeTimestamp(char* buf, size_t len, unsigned long ms) {
    unsigned long totalSeconds = ms / 1000;
    snprintf(buf, len, "%02lu:%02lu:%02lu.%03lu",
             (totalSeconds / 3600) % 24,
             (totalSeconds / 60) % 60,
             totalSeconds % 60,
             ms % 1000);
}

void log_to_buffer(const char* level, const char* format, ...) {
    unsigned long nowMs = millis();
    va_list args;
    va_start(args, format);

    char ts[16];
#if LOG_HUMANIZED
    humanizeTimestamp(ts, sizeof(ts), nowMs);
    snprintf(logBuffer[logHead].message, LOG_MESSAGE_MAX_LEN,
             "[%s] %s | ", level, ts);
#else
    snprintf(logBuffer[logHead].message, LOG_MESSAGE_MAX_LEN,
             "[%s] %lu | ", level, nowMs);
#endif

    vsnprintf(logBuffer[logHead].message + strlen(logBuffer[logHead].message),
              LOG_MESSAGE_MAX_LEN - strlen(logBuffer[logHead].message),
              format, args);
    strlcat(logBuffer[logHead].message, "\n", LOG_MESSAGE_MAX_LEN);

    va_end(args);

    noInterrupts();
    logHead = (logHead + 1) % LOG_BUFFER_SIZE;
    if (logCount < LOG_BUFFER_SIZE) logCount++;
    interrupts();
}

#define LOG_W(...) log_to_buffer("WARN ", __VA_ARGS__)
#define LOG_I(...) log_to_buffer("INFO ", __VA_ARGS__)
#define LOG_E(...) log_to_buffer("ERROR", __VA_ARGS__)
#define LOG_D(...) log_to_buffer("DEBUG", __VA_ARGS__)

// ==========================================================
// 2. HARDWARE PINS & CONSTANTS
// ==========================================================
#define FIRMWARE_VERSION "MCSS_v1"
#define ENABLE_DEBUG

constexpr int TOTAL_FLOORS = 6;

// OLED
constexpr int SCREEN_WIDTH   = 128;
constexpr int SCREEN_HEIGHT  = 64;
constexpr int OLED_RESET     = -1;
constexpr uint8_t SCREEN_ADDRESS = 0x3C;

// Pintu Relay
constexpr int RELAY_OPEN         = 26;
constexpr int RELAY_CLOSE        = 25;
constexpr int DOOR_SENSOR_CLOSED = 18;  // LOW = active
constexpr int DOOR_SENSOR_OPEN   = 19;  // LOW = active

// Kabin Relay
constexpr int RELAY_CAR_UP   = 14;
constexpr int RELAY_CAR_DOWN = 27;

// Limit Switch Lantai (LOW = active)
constexpr int CAR_LS_FLOOR_1 = 33;
constexpr int CAR_LS_FLOOR_2 = 32;
constexpr int CAR_LS_FLOOR_3 = 35;
constexpr int CAR_LS_FLOOR_4 = -4;
constexpr int CAR_LS_FLOOR_5 = -5;
constexpr int CAR_LS_FLOOR_6 = -6;

const int CAR_LS_PINS[TOTAL_FLOORS + 1] = {
    0, CAR_LS_FLOOR_1, CAR_LS_FLOOR_2, CAR_LS_FLOOR_3,
    CAR_LS_FLOOR_4, CAR_LS_FLOOR_5, CAR_LS_FLOOR_6
};

static_assert(sizeof(CAR_LS_PINS) / sizeof(CAR_LS_PINS[0]) == TOTAL_FLOORS + 1);

// Safety & Control
constexpr int SAFETY_CHAIN_IN        = 36;
constexpr int MODE_AUTO_SEL          = 39;
constexpr int MODE_INSP_SEL          = 34;
constexpr int MANUAL_CAR_JOG_UP_PIN  = 23;
constexpr int MANUAL_CAR_JOG_DOWN_PIN= 5;
constexpr int BUZZER_PIN             = 15;
constexpr int LED_HEARTBEAT          = 2;
constexpr int LIGHT_CABIN_PIN        = 13;

// ==========================================================
// 3. TIMING CONSTANTS
// ==========================================================
constexpr unsigned long CAR_MAX_MOVE_DURATION_MS    = 30000;
constexpr unsigned long OLED_UPDATE_INTERVAL_MS     = 200;
constexpr unsigned long HEARTBEAT_INTERVAL_MS       = 500;
constexpr unsigned long DOOR_DWELL_TIME_MS          = 6000;
constexpr unsigned long SOFT_FAULT_BEEP_DURATION_MS = 1000;
constexpr unsigned long JOG_TIMEOUT_MS              = 500;
constexpr unsigned long MODE_DEBOUNCE_MS            = 50;
constexpr unsigned long LOG_THROTTLE_MS             = 5000;

// Relay-specific timing
constexpr unsigned long DOOR_DEBOUNCE_MS            = 30;
constexpr unsigned long DOOR_DIRECTION_DEAD_TIME_MS = 60;
constexpr unsigned long DOOR_EARLY_MOTION_CHECK_MS  = 2000;
constexpr unsigned long DOOR_TOTAL_MOTION_MAX_MS    = 30000;
constexpr unsigned long DOOR_LS_SETTLE_DELAY_MS     = 400;

// Tambahan patch untuk konsistensi FSM
constexpr unsigned long DOOR_EXTENDED_TIMEOUT_MS    = 8000; // batas per arah (contoh: 8 detik)
constexpr uint8_t       DOOR_MAX_RETRY              = 3;    // jumlah retry maksimum
constexpr unsigned long HARD_FAULT_RESET_HOLD_MS = 3000; // contoh: 3 detik
constexpr unsigned long DEBOUNCE_TIME_MS = 50;

// ==========================================================
// 4. ENUMS
// ==========================================================
enum DoorState {
    DOOR_CLOSED,
    DOOR_OPEN,
    DOOR_MOVING_TO_OPEN,
    DOOR_MOVING_TO_CLOSE,
    DOOR_CHANGING_DIRECTION,
    DOOR_ERROR,
    DOOR_HOMING_TO_CLOSE,
    DOOR_STOPPED_MID_MOVE,
    DOOR_MANUAL_JOG
};

enum CarState {
    CAR_IDLE,
    CAR_MOVING_UP,
    CAR_MOVING_DOWN,
    CAR_WAITING_DOOR_OPEN,
    CAR_ERROR_TIMEOUT
};

enum SystemState {
    SYS_SAFE_STATE,
    SYS_INIT_DOOR_HOMING,
    SYS_INIT_CAR_HOMING,
    SYS_READY_AUTO
};

enum Mode      { MODE_OFF, MODE_AUTO, MODE_INSPECTION };
enum Direction { DIR_IDLE, DIR_UP, DIR_DOWN };
enum FSMAction { FSM_IDLE, FSM_DOOR, FSM_CAR, FSM_SYSTEM };

FSMAction last_fsm_action_logged = FSM_IDLE;

// ==========================================================
// 5. GLOBAL STATE
// ==========================================================
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Core status
Mode        currentMode       = MODE_OFF;
int         currentFloor      = 0;
int         targetFloor       = 0;
uint32_t    pending_calls     = 0;
uint8_t     callAge[TOTAL_FLOORS + 1] = {0};
constexpr uint8_t AGE_FORCE_SERVE     = 10;

// Movement & position
volatile bool isCarMoving         = false;
Direction     currentDirection    = DIR_IDLE;
bool          isCarPositionKnown  = false;

// FSM states
SystemState currentSystemState = SYS_SAFE_STATE;
CarState    currentCarState    = CAR_IDLE;
DoorState   currentDoorState   = DOOR_CLOSED;

// Safety & indicators
bool safetyOK          = true;
bool criticalError     = false;
bool isLightOn         = false;
bool isOverloaded      = false;
unsigned long faultStartTime    = 0;
String        lastFaultCode     = "";
unsigned long totalFaultCount   = 0;

// ISR & floor tracking
volatile unsigned long floorIsrTriggerTime[TOTAL_FLOORS + 1] = {0};
volatile bool          floorIsrFlag[TOTAL_FLOORS + 1]        = {false};
bool                   lastLimitStatus[TOTAL_FLOORS + 1]     = {false};

// Timing variables (relay-specific)
unsigned long doorMoveStartTime      = 0;
unsigned long doorDwellStartTime     = 0;
unsigned long carMoveStartTime       = 0;
unsigned long doorSettleStartTime    = 0;
unsigned long directionChangeStart   = 0;
unsigned long lastOledUpdate         = 0;
unsigned long lastHeartbeat          = 0;
unsigned long lastModeReadTime       = 0;
unsigned long lastJogReceivedMs      = 0;
unsigned long buzzerSoftFaultEndTime = 0;
unsigned long hardFaultResetHoldStartTime = 0;

// Relay door control
bool      doorTargetOpen         = false;
uint32_t  lastStableOpenTime     = 0;
uint32_t  lastStableCloseTime    = 0;
uint8_t   doorOpenRetryCount         = 0; 
uint8_t   doorCloseRetryCount         = 0;     // Ganti jadi satu variabel (simetris open/close)
uint32_t  doorTotalMotionStart   = 0;
uint32_t  doorStoppedStartTime   = 0;
bool      earlyMotionChecked     = false;
bool      doorRecoveryLogged     = false;
bool      doorSettleLogged = false;
bool isDoorJogOpenHeld   = false;
bool isDoorJogCloseHeld  = false;

// Jog (tombol fisik car saja, pintu dari RS485)
bool isJogUpHeld   = false;
bool isJogDownHeld = false;

// Arrival beep
struct ArrivalBeep {
    int count = 0;
    unsigned long next = 0;
    bool state = false;
} arrivalBeep;

// Logging guards (bisa dikurangi jika tidak semua dipakai)
bool carInterlockLogged     = false;
bool carStartLogged         = false;
bool fsmClosingDoorLogged   = false;
bool safetyFaultLogged      = false;
bool criticalErrorLogged    = false;
bool carHomingLogged        = false;

// ==========================================================
// === 7. RS-485 LEGACY COUNTERS (STANDALONE MODE)        ===
// ==========================================================
unsigned long lastSuccessfulCommMs = 0;
unsigned long rs485LossCount       = 0;
unsigned long totalPacketsSent     = 0;
unsigned long totalPacketsReceived = 0;
unsigned long totalCrcErrors       = 0;
unsigned long totalRetries         = 0;

// ==========================
// Konstanta Protokol
// ==========================

// --- Build-time behavior toggles ---
#define RS485_USE_FLUSH 1   // 1: pakai Serial2.flush() (safety-first), 0: non-blocking (determinisme loop)

constexpr unsigned long BAUDRATE                = 19200;
constexpr uint8_t       START_BYTE              = 0xAA;
constexpr uint8_t       MASTER_ADDRESS          = 0x01;
constexpr uint8_t       SLAVE_ADDRESS           = 0x04;
constexpr size_t        PACKET_SIZE             = 8;

// ==========================
// Indeks Paket
// ==========================
constexpr uint8_t PKT_IDX_START     = 0;
constexpr uint8_t PKT_IDX_ADDR      = 1;
constexpr uint8_t PKT_IDX_CMD       = 2;
constexpr uint8_t PKT_IDX_FLOOR     = 3;
constexpr uint8_t PKT_IDX_MODE      = 4;
constexpr uint8_t PKT_IDX_DIR_FAULT = 5;
constexpr uint8_t PKT_IDX_CRC_LO    = 6;
constexpr uint8_t PKT_IDX_CRC_HI    = 7;

// ==========================
// Command Codes
// ==========================
constexpr uint8_t CMD_POLL_STATUS   = 0x10;
constexpr uint8_t CMD_CAR_CALL      = 0x20;
constexpr uint8_t CMD_DOOR_OPEN     = 0x30;
constexpr uint8_t CMD_DOOR_CLOSE    = 0x31;
constexpr uint8_t CMD_JOG_DOOR_OPEN = 0x40;
constexpr uint8_t CMD_JOG_DOOR_CLOSE= 0x41;
constexpr uint8_t CMD_EMERGENCY     = 0x50;
constexpr uint8_t CMD_LIGHT_TOGGLE  = 0x60;
constexpr uint8_t ACK               = 0x70;

// ==========================
// Timing Constants
// ==========================
constexpr unsigned long COMM_UPDATE_INTERVAL_MS = 200;
constexpr unsigned long COMM_TIMEOUT_MS         = 100;
constexpr uint8_t       MAX_CONSECUTIVE_ERRORS  = 5;
constexpr unsigned long GUARD_TIME_US           = 100; // fallback minimum guard time

// ==========================
// Pin Definitions
// ==========================
constexpr uint8_t RS485_DE_RE       = 4;
constexpr uint8_t RS485_RX_PIN      = 16;
constexpr uint8_t RS485_TX_PIN      = 17;

// Buffer & state
static uint8_t rxBuffer[PACKET_SIZE];
static uint8_t rxIndex = 0;
static unsigned long sendGuardStart = 0;
static unsigned long waitStartMs = 0;
static unsigned long lastCommUpdate = 0;
static bool commLost = false;
static unsigned long lastRecoveryAttemptMs = 0;

enum CommState : uint8_t { COMM_IDLE, COMM_WAIT_RESPONSE };
static CommState currentCommState = COMM_IDLE;

// Statistik komunikasi
struct CommStats {
    uint32_t totalPacketsReceived = 0;
    uint32_t consecutiveCommErrors = 0;
    unsigned long lastSuccessfulCommMs = 0;
} commStats;

// ===============================================
// === 5. PROTOTYPES ===
const char* modeToString(Mode m);
const char* doorStateToString(DoorState ds);
const char* carStateToString(CarState cs);
const char* systemStateToString(SystemState ss);
uint16_t calculateCRC(const uint8_t* data, size_t len);

void        stopDoorRelay();
void        startDirectionChange(bool openDirection);
void        doorInitializePosition();
void        controlDoor(bool);

void        carInit();
void        controlCarMovement(Direction dir);
bool        isOpenLimitActive();
bool        isCloseLimitActive();

void        handleSystemModeChange();
void        handleDoorFSM();
void        handleAutoModeFSM();
void        handleSafeState();
void        handleDoorHoming();
void        handleCarHoming();
void        agePendingCalls();
int         getNextTargetFromQueue();
void        addCallToQueue(int floor);
void        handleReadyAuto();

void        checkFloorLimitSensors();
void        handleCriticalErrorState();
void        handleHardFaultReset();
void        triggerArrivalBeep();
void        serviceArrivalBeep();
void        updateIndicators();
void        updateFaultCounter(const char* code);
void        displayCriticalError();
void        updateOLED();

void        handleSerialMonitor();
void        printHelp();

void        isr_floor_1();
void        isr_floor_2();
void        isr_floor_3();

void        rs485MasterInit();
void        handleRS485Master();
void        processCommand(uint8_t* packet);
void        sendRS485Packet(const uint8_t* data, size_t length);

void        maintenanceUpdate();

const char* faultCodeToString();
const char* faultDescription(const char* code);

// ===============================================
// === 4. INTERRUPT SERVICE ROUTINES (ISR) ===
// ===============================================

void isr_floor_1() {
    floorIsrTriggerTime[1] = millis();
    floorIsrFlag[1] = true;
}

void isr_floor_2() {
    floorIsrTriggerTime[2] = millis();
    floorIsrFlag[2] = true;
}

void isr_floor_3() {
    floorIsrTriggerTime[3] = millis();
    floorIsrFlag[3] = true;
}

// ===============================================
// === 5. IMPLEMENTASI FUNGSI UTILITY & KONTROL ===
// ===============================================
// CRC16 Modbus (polynomial 0xA001, init 0xFFFF)
uint16_t calculateCRC(const uint8_t* data, size_t len) {
    uint16_t crc = 0xFFFF;

    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }

    return crc;
}

const char* modeToString(Mode m) {
    switch (m) {
        case MODE_OFF: return "OFF";
        case MODE_AUTO: return "AUTO";
        case MODE_INSPECTION: return "INSPEKSI";
        default: return "UNKNOWN";
    }
}

const char* doorStateToString(DoorState ds) {
    switch (ds) {
        case DOOR_CLOSED: return "TUTUP";
        case DOOR_OPEN: return "BUKA";
        case DOOR_MOVING_TO_OPEN: return "M_BUKA";
        case DOOR_MOVING_TO_CLOSE: return "M_TUTUP";
        case DOOR_ERROR: return "ERROR";
        case DOOR_HOMING_TO_CLOSE: return "HOMING";
        case DOOR_STOPPED_MID_MOVE: return "STOP_MID";
        case DOOR_MANUAL_JOG: return "JOG";
        default: return "N/A";
    }
}

const char* carStateToString(CarState cs) {
    switch (cs) {
        case CAR_IDLE: return "IDLE";
        case CAR_MOVING_UP: return "NAIK";
        case CAR_MOVING_DOWN: return "TURUN";
        case CAR_WAITING_DOOR_OPEN: return "WAIT_O";
        case CAR_ERROR_TIMEOUT: return "ERROR";
        default: return "N/A";
    }
}

const char* systemStateToString(SystemState ss) {
    switch (ss) {
        case SYS_SAFE_STATE: return "SAFE_STATE"; 
        case SYS_INIT_DOOR_HOMING: return "INIT_D";
        case SYS_INIT_CAR_HOMING: return "INIT_C";
        case SYS_READY_AUTO: return "READY";
        default: return "SYS_N/A";
    }
}

const char* faultCodeToString() {
    if (isOverloaded) return "OVL";
    if (!safetyOK) return "SAFE";
    if (digitalRead(DOOR_SENSOR_OPEN) == LOW &&
        digitalRead(DOOR_SENSOR_CLOSED) == LOW) return "DRM";
    if (currentDoorState != DOOR_CLOSED) return "DIL";
    if (currentCarState == CAR_ERROR_TIMEOUT) return "CMT";
    if (currentSystemState != SYS_INIT_CAR_HOMING) return "HFL";
    if (currentMode == MODE_OFF) return "MODE_OFF";
    return "UNK";
}

const char* faultDescription(const char* code) {
    if (strcmp(code, "OVL") == 0) return "Overload";
    if (strcmp(code, "SAFE") == 0) return "Safety Chain Broken";
    if (strcmp(code, "DRM") == 0) return "Door Sensor Mismatch";
    if (strcmp(code, "EMG") == 0) return "Emergency Stop";
    if (strcmp(code, "DIL") == 0) return "Door Interlock Fault";
    if (strcmp(code, "CMT") == 0) return "Car Move Timeout";
    if (strcmp(code, "HFL") == 0) return "Homing Failed";
    if (strcmp(code, "MODE_OFF") == 0) return "MCC_OFFLINE";
    return "Unknown Fault";
}

// === Throttled logging ===
bool logThrottled(const char* fmt, ...) {
    static unsigned long lastLogMs = 0;
    if (millis() - lastLogMs < LOG_THROTTLE_MS) return false;
    lastLogMs = millis();

    va_list args;
    va_start(args, fmt);
    char temp[LOG_MESSAGE_MAX_LEN];
    vsnprintf(temp, sizeof(temp), fmt, args);
    va_end(args);

    log_to_buffer("THROTTLE", "%s", temp);
    return true;
}

// === Drain buffer di loop utama ===
void processLogBuffer() {
    if (logCount > 0) {
        Serial.print(logBuffer[logTail].message);
        noInterrupts();
        logTail = (logTail + 1) % LOG_BUFFER_SIZE;
        logCount--;
        interrupts();
    }
}

// ==========================================================
// === FUNGSI KONTROL CAR ===
// ==========================================================

void carInit() {
    if (digitalRead(CAR_LS_FLOOR_1) == LOW) {
        isCarPositionKnown = true;
        currentFloor = 1;
        targetFloor = 1;
        lastLimitStatus[1] = true;
        LOG_I("CAR POS: Initialized at F1.");
    } else {
        isCarPositionKnown = false;
        targetFloor = 1;
        LOG_I("CAR POS: Unknown (Will attempt homing).");
    }
}

void controlCarMovement(Direction dir) {
    bool canMove = true;

    // ======================================================
    // INTERLOCK LOGIC
    // ======================================================

    if (dir != DIR_IDLE && currentMode == MODE_AUTO) {
    if (currentDoorState != DOOR_CLOSED) {
        canMove = false;

        if (currentDoorState != DOOR_MOVING_TO_CLOSE &&
            currentDoorState != DOOR_HOMING_TO_CLOSE &&
            currentDoorState != DOOR_ERROR) {

            if (!carInterlockLogged) {
                LOG_E("CAR INTERLOCK: Movement %s cancelled. Door FSM is %s (Not CLOSED).",
                      dir == DIR_UP ? "UP" : "DOWN",
                      doorStateToString(currentDoorState));
                carInterlockLogged = true;
            }
        } 
        else if (currentDoorState == DOOR_ERROR && isCarMoving) {
            if (!carInterlockLogged) {
                LOG_E("CAR INTERLOCK: Movement %s cancelled. Door FSM in CRITICAL ERROR.",
                      dir == DIR_UP ? "UP" : "DOWN");
                carInterlockLogged = true;
            }
        }
    }
}

// Reset flag ketika interlock tidak berlaku
if (canMove) {
    carInterlockLogged = false;
}

    if (currentMode != MODE_INSPECTION) {
        if (digitalRead(DOOR_SENSOR_OPEN) == LOW &&
            digitalRead(DOOR_SENSOR_CLOSED) == LOW) {

            canMove = false;
            criticalError = true;

            static unsigned long lastCrit = 0;
            if (millis() - lastCrit > 5000) {
                LOG_E("CRITICAL: Sensor Pintu Korslet! (Keduanya aktif)");
                lastCrit = millis();
            }
        }

        if (digitalRead(DOOR_SENSOR_OPEN) == LOW ||
            currentDoorState != DOOR_CLOSED) {
            canMove = false;
        }
    }

    if (!canMove) dir = DIR_IDLE;

    // ======================================================
    // MOTOR EXECUTION
    // ======================================================

    if (dir == DIR_UP) {
        digitalWrite(RELAY_CAR_UP, HIGH);
        digitalWrite(RELAY_CAR_DOWN, LOW);

        if (!isCarMoving) {
            carMoveStartTime = millis();
            LOG_I("CAR START: Moving UP. Target: F%d (Mode: %s)",
                  targetFloor, modeToString(currentMode));
            carStartLogged = true;
        }
        isCarMoving = true;
    }
    else if (dir == DIR_DOWN) {
        digitalWrite(RELAY_CAR_UP, LOW);
        digitalWrite(RELAY_CAR_DOWN, HIGH);

        if (!isCarMoving) {
            carMoveStartTime = millis();
            LOG_I("CAR START: Moving DOWN. Target: F%d (Mode: %s)",
                  targetFloor, modeToString(currentMode));
            carStartLogged = true;
        }
        isCarMoving = true;
    }
    else { // DIR_IDLE
        if (isCarMoving) {
            LOG_I("CAR STOPPED. Final Floor: %d (Pos Known: %s)",
                  currentFloor, isCarPositionKnown ? "YES" : "NO");
        }
        digitalWrite(RELAY_CAR_UP, LOW);
        digitalWrite(RELAY_CAR_DOWN, LOW);

        isCarMoving = false;
        carMoveStartTime = 0;
        carStartLogged = false; // reset flag saat berhenti
    }

    currentDirection = dir;
}

// ==========================================================
// === FUNGSI KONTROL PINTU ===
// ==========================================================

// Panggil sekali di setup()
void doorInit() {
    lastStableOpenTime  = millis();
    lastStableCloseTime = millis();
    stopDoorRelay();
    currentDoorState = DOOR_CLOSED;  // asumsi awal aman
}

void doorInitializePosition() {
    // Pastikan relay mati total di awal (safety)
    stopDoorRelay();

    // Baca sensor dengan debounce yang sudah ada (lebih aman daripada digitalRead langsung)
    bool ls_open  = isOpenLimitActive();
    bool ls_close = isCloseLimitActive();

    // ── Kasus kritis: kedua sensor aktif (short / wiring fault) ─────────────
    if (ls_open && ls_close) {
        criticalError = true;
        currentDoorState = DOOR_ERROR;
        LOG_E("SETUP CRITICAL: Both limit switches ACTIVE at startup! (wiring/short fault)");
        // Opsional: tambah indikator error (LED blink, buzzer, dll)
        return;
    }
}

// ==========================================================
//   SENSOR + DEBOUNCE
// ==========================================================
bool isOpenLimitActive() {
    bool reading = (digitalRead(DOOR_SENSOR_OPEN) == LOW);
    if (reading && millis() - lastStableOpenTime >= DOOR_DEBOUNCE_MS) return true;
    if (!reading) lastStableOpenTime = millis();
    return false;
}

bool isCloseLimitActive() {
    bool reading = (digitalRead(DOOR_SENSOR_CLOSED) == LOW);
    if (reading && millis() - lastStableCloseTime >= DOOR_DEBOUNCE_MS) return true;
    if (!reading) lastStableCloseTime = millis();
    return false;
}

// ==========================================================
//   HARDWARE ABSTRACTION
// ==========================================================
void stopDoorRelay() {
    digitalWrite(RELAY_OPEN,  LOW);
    digitalWrite(RELAY_CLOSE, LOW);
}

void startDirectionChange(bool openDirection) {
    stopDoorRelay();
    doorTargetOpen = openDirection;
    directionChangeStart = millis();
    doorSettleLogged = false;           // reset settle setiap kali mulai gerak baru
}

// ==========================================================
//   KONTROL PINTU (dipanggil dari luar: true = buka, false = tutup)
// ==========================================================
void controlDoor(bool shouldOpen) {
    if (currentMode == MODE_OFF && currentDoorState != DOOR_HOMING_TO_CLOSE) {
        stopDoorRelay();
        currentDoorState = DOOR_CLOSED;
        doorTotalMotionStart = 0;
        return;
    }

    bool ls_open  = isOpenLimitActive();
    bool ls_close = isCloseLimitActive();

    // Interlock: command berlawanan saat sedang gerak → stop dulu
    if ((currentDoorState == DOOR_MOVING_TO_OPEN  && !shouldOpen) ||
        (currentDoorState == DOOR_MOVING_TO_CLOSE &&  shouldOpen)) {
        stopDoorRelay();
        currentDoorState = DOOR_STOPPED_MID_MOVE;
        doorStoppedStartTime = millis();
        doorMoveStartTime = 0;
        LOG_I("DOOR: Direction conflict → STOPPED_MID_MOVE");
        return;
    }

    // Sudah di posisi target → langsung akhiri
    if ((shouldOpen && (currentDoorState == DOOR_OPEN || ls_open)) ||
        (!shouldOpen && (currentDoorState == DOOR_CLOSED || ls_close))) {
        stopDoorRelay();
        currentDoorState = shouldOpen ? DOOR_OPEN : DOOR_CLOSED;
        if (shouldOpen) doorOpenRetryCount = 0;
        else            doorCloseRetryCount = 0;
        if (shouldOpen && doorDwellStartTime == 0) doorDwellStartTime = millis();
        doorTotalMotionStart = 0;
        return;
    }

    startDirectionChange(shouldOpen);
    currentDoorState = DOOR_CHANGING_DIRECTION;
    doorMoveStartTime = millis();
    earlyMotionChecked = false;

    if (doorTotalMotionStart == 0) doorTotalMotionStart = millis();

    LOG_I("DOOR: %s commanded", shouldOpen ? "OPEN" : "CLOSE");
}

// ==========================================================
//   FSM – panggil rutin (disarankan tiap 20–100 ms)
// ==========================================================
void handleDoorFSM() {
    bool ls_open  = isOpenLimitActive();
    bool ls_close = isCloseLimitActive();

    // ── Critical: kedua sensor aktif ───────────────────────────────
    if (ls_open && ls_close) {
        stopDoorRelay();
        currentDoorState = DOOR_ERROR;
        criticalError = true;
        LOG_E("DOOR CRITICAL: Both limit switches ACTIVE");
        doorTotalMotionStart = 0;
        return;
    }

    // ── Auto-recovery dari ERROR ───────────────────────────────────
    if (currentDoorState == DOOR_ERROR) {
        if (ls_close && !ls_open) {
            stopDoorRelay();
            currentDoorState = DOOR_CLOSED;
            criticalError = false;
            doorCloseRetryCount = doorOpenRetryCount = 0;
            doorTotalMotionStart = 0;
            if (!doorRecoveryLogged) {
                LOG_I("DOOR: Recovered → CLOSED");
                doorRecoveryLogged = true;
            }
        }
        else if (ls_open && !ls_close) {
            stopDoorRelay();
            currentDoorState = DOOR_OPEN;
            criticalError = false;
            doorCloseRetryCount = doorOpenRetryCount = 0;
            doorTotalMotionStart = 0;
            if (!doorRecoveryLogged) {
                LOG_I("DOOR: Recovered → OPEN");
                doorRecoveryLogged = true;
            }
        }
        return;
    }
    doorRecoveryLogged = false;

    if (currentMode == MODE_OFF) return;

    // ── Non-blocking dead-time antar relay ─────────────────────────
    if (currentDoorState == DOOR_CHANGING_DIRECTION) {
        if (millis() - directionChangeStart >= DOOR_DIRECTION_DEAD_TIME_MS) {
            if (doorTargetOpen) {
                digitalWrite(RELAY_OPEN, HIGH);
                digitalWrite(RELAY_CLOSE, LOW);
                currentDoorState = DOOR_MOVING_TO_OPEN;
            } else {
                digitalWrite(RELAY_OPEN, LOW);
                digitalWrite(RELAY_CLOSE, HIGH);
                currentDoorState = DOOR_MOVING_TO_CLOSE;
            }
        }
        return;
    }

    // ── Logika saat sedang bergerak ────────────────────────────────
    if (currentDoorState == DOOR_MOVING_TO_OPEN || currentDoorState == DOOR_MOVING_TO_CLOSE) {
        uint32_t elapsed      = millis() - doorMoveStartTime;
        uint32_t totalElapsed = doorTotalMotionStart ? millis() - doorTotalMotionStart : 0;

        // Total gerak melebihi batas → force error (proteksi mekanik)
        if (totalElapsed > DOOR_TOTAL_MOTION_MAX_MS) {
            stopDoorRelay();
            currentDoorState = DOOR_ERROR;
            criticalError = true;
            LOG_E("DOOR: TOTAL motion timeout (>30s) – possible jam");
            doorTotalMotionStart = 0;
            return;
        }

        // Single direction timeout
        if (elapsed > DOOR_EXTENDED_TIMEOUT_MS) {
            stopDoorRelay();
            currentDoorState = DOOR_ERROR;
            criticalError = true;
            LOG_E("DOOR: Single direction timeout");
            return;
        }

        // ── Early motion detection (lewati jika sudah di ujung target) ──
        bool alreadyAtTarget = (currentDoorState == DOOR_MOVING_TO_OPEN  && ls_open) ||
                               (currentDoorState == DOOR_MOVING_TO_CLOSE && ls_close);

        if (!earlyMotionChecked && elapsed >= DOOR_EARLY_MOTION_CHECK_MS && !alreadyAtTarget) {
            earlyMotionChecked = true;

            bool stillAtStart = (currentDoorState == DOOR_MOVING_TO_OPEN) ? !ls_open : !ls_close;
            if (stillAtStart) {
                LOG_W("DOOR: No early motion in %d ms – possible mech slip", DOOR_EARLY_MOTION_CHECK_MS);

                uint8_t& retry = (currentDoorState == DOOR_MOVING_TO_CLOSE) ? doorCloseRetryCount : doorOpenRetryCount;
                if (retry < DOOR_MAX_RETRY) {
                    retry++;
                    startDirectionChange(currentDoorState == DOOR_MOVING_TO_OPEN);
                    currentDoorState = DOOR_CHANGING_DIRECTION;
                    return;
                }
            }
        }

        // ── Sampai di target ───────────────────────────────────────
        bool atTarget = (currentDoorState == DOOR_MOVING_TO_CLOSE) ? ls_close : ls_open;

        if (atTarget) {
            stopDoorRelay();
            currentDoorState = (currentDoorState == DOOR_MOVING_TO_CLOSE) ? DOOR_CLOSED : DOOR_OPEN;
            doorCloseRetryCount = doorOpenRetryCount = 0;
            doorTotalMotionStart = 0;
            if (currentDoorState == DOOR_OPEN && doorDwellStartTime == 0) {
                doorDwellStartTime = millis();
                LOG_I("DOOR: Opened – dwell started");
            } else if (currentDoorState == DOOR_CLOSED) {
                LOG_I("DOOR: Closed confirmed");
            }
            return;
        }

        // ── Settle + retry logic ───────────────────────────────────
        if (elapsed > 2500 && !doorSettleLogged) {
            doorSettleStartTime = millis();
            doorSettleLogged = true;
        }

        if (doorSettleLogged && millis() - doorSettleStartTime > DOOR_LS_SETTLE_DELAY_MS) {
            if (atTarget) {
                stopDoorRelay();
                currentDoorState = (currentDoorState == DOOR_MOVING_TO_CLOSE) ? DOOR_CLOSED : DOOR_OPEN;
                doorSettleLogged = false;
                doorCloseRetryCount = doorOpenRetryCount = 0;
                doorTotalMotionStart = 0;
                return;
            }

            uint8_t& retry = (currentDoorState == DOOR_MOVING_TO_CLOSE) ? doorCloseRetryCount : doorOpenRetryCount;
            if (retry < DOOR_MAX_RETRY) {
                retry++;
                LOG_W("DOOR: %s sensor not active – retry %d/%d",
                      currentDoorState == DOOR_MOVING_TO_CLOSE ? "Close" : "Open",
                      retry, DOOR_MAX_RETRY);
                startDirectionChange(currentDoorState == DOOR_MOVING_TO_OPEN);
                currentDoorState = DOOR_CHANGING_DIRECTION;
                return;
            }
        }
    }

    // ── STOPPED_MID_MOVE timeout ───────────────────────────────────
    if (currentDoorState == DOOR_STOPPED_MID_MOVE) {
        if (millis() - doorStoppedStartTime > 5000) {
            currentDoorState = DOOR_ERROR;
            LOG_W("DOOR: STOPPED_MID_MOVE timeout → ERROR");
        }
    }

    // ── Auto close setelah dwell ───────────────────────────────────
    if (currentDoorState == DOOR_OPEN && doorDwellStartTime != 0 &&
        currentMode == MODE_AUTO && currentSystemState == SYS_READY_AUTO) {
        if (millis() - doorDwellStartTime > DOOR_DWELL_TIME_MS) {
            LOG_I("DOOR: Dwell ended → auto closing");
            controlDoor(false);
            doorDwellStartTime = 0;
        }
    }
}

// ==========================================================
// === QUEUE ===
// ==========================================================
void agePendingCalls() {
    for (int f = 1; f <= TOTAL_FLOORS; f++) {
        if (pending_calls & (1 << f)) {
            if (callAge[f] < 255) callAge[f]++;
        } else {
            callAge[f] = 0;
        }
    }
}

int getNextTargetFromQueue() {

    if (pending_calls == 0) return 0;

    // ========================================================
    // PRIORITAS 0: CALL TERLALU LAMA (FORCE SERVE)
    // Strategi: Cari yang terdekat di antara yang sudah "tua"
    // ========================================================
    int bestAgedFloor = 0;
    int bestAgedDist = TOTAL_FLOORS + 1;

    for (int f = 1; f <= TOTAL_FLOORS; f++) {
        if ((pending_calls & (1 << f)) && callAge[f] >= AGE_FORCE_SERVE) {
            int d = abs(f - currentFloor);
            
            // Penalti arah (optional): Jika harus balik arah, anggap lebih jauh sedikit
            int penalty = 0;
            if ((currentDirection == DIR_UP && f < currentFloor) ||
                (currentDirection == DIR_DOWN && f > currentFloor)) {
                penalty = 1; 
            }

            if ((d + penalty) < bestAgedDist) {
                bestAgedDist = d + penalty;
                bestAgedFloor = f;
            }
        }
    }
    
    // Jika ditemukan lantai yang sudah tua, layani segera
    if (bestAgedFloor > 0) return bestAgedFloor;

    // ========================================================
    // PRIORITAS 1: LOOK ALGORITHM (Normal Movement)
    // Strategi: Selesaikan arah yang sekarang sebelum balik arah
    // ========================================================
    if (currentDirection == DIR_UP) {
        // Lanjut ke atas dulu
        for (int f = currentFloor + 1; f <= TOTAL_FLOORS; f++) {
            if (pending_calls & (1 << f)) return f;
        }
        // Baru balik ke bawah jika atas sudah kosong
        for (int f = currentFloor - 1; f >= 1; f--) {
            if (pending_calls & (1 << f)) return f;
        }
    }
    else if (currentDirection == DIR_DOWN) {
        // Lanjut ke bawah dulu
        for (int f = currentFloor - 1; f >= 1; f--) {
            if (pending_calls & (1 << f)) return f;
        }
        // Baru balik ke atas jika bawah sudah kosong
        for (int f = currentFloor + 1; f <= TOTAL_FLOORS; f++) {
            if (pending_calls & (1 << f)) return f;
        }
    }

    // ========================================================
    // PRIORITAS 2: IDLE → TERDEKAT (Heuristic Optimization)
    // Strategi: Digunakan saat lift baru start atau state ambigu
    // ========================================================
    int bestFloor = 0;
    int bestScore = 100000;

    for (int f = 1; f <= TOTAL_FLOORS; f++) {
        if (pending_calls & (1 << f)) {
            int dist  = abs(f - currentFloor);
            // Heuristik: Jarak punya bobot lebih besar dari usia panggilan
            int score = dist * 4 - callAge[f]; 

            if (score < bestScore) {
                bestScore = score;
                bestFloor = f;
            }
        }
    }

    return bestFloor;
}

void addCallToQueue(int floor) {

    if (criticalError) return;
    if (floor < 1 || floor > TOTAL_FLOORS) return;

    if (!(pending_calls & (1 << floor))) {
        pending_calls |= (1 << floor);
        callAge[floor] = 0;
        LOG_I("QUEUE: Added F%d", floor);
    }

    // Immediate open tetap
    if (floor == currentFloor &&
        currentCarState == CAR_IDLE &&
        currentDoorState == DOOR_CLOSED &&
        currentMode == MODE_AUTO &&
        currentSystemState == SYS_READY_AUTO)
    {
        pending_calls &= ~(1 << floor);
        callAge[floor] = 0;
        currentCarState = CAR_WAITING_DOOR_OPEN;
        controlDoor(true);
    }
}

// ==========================================================
// === FUNGSI FSM MODE OTOMATIS (REVISI LOGIKA BUKA PINTU) ===
// ==========================================================

// ==== FSM Sub-Fungsi ====
void handleSafeState() {
    if (safetyOK && !criticalError) {
        currentSystemState = (!isCarPositionKnown || currentDoorState != DOOR_CLOSED)
                             ? SYS_INIT_DOOR_HOMING
                             : SYS_READY_AUTO;
        currentCarState = CAR_IDLE;
    }
}

void handleDoorHoming() {
    if (currentDoorState != DOOR_CLOSED && currentDoorState != DOOR_HOMING_TO_CLOSE) {
        currentDoorState = DOOR_HOMING_TO_CLOSE;
        controlDoor(false);
        doorMoveStartTime = millis();
    } else if (currentDoorState == DOOR_CLOSED) {
        currentSystemState = SYS_INIT_CAR_HOMING;
    } else {
        controlCarMovement(DIR_IDLE);
    }
}

void handleCarHoming() {
    if (isCarPositionKnown) {
        controlCarMovement(DIR_IDLE);
        targetFloor = currentFloor;
        currentSystemState = SYS_READY_AUTO;
        currentCarState = CAR_IDLE;

        if (carHomingLogged) {
            LOG_I("FSM: Car Homing finished. System READY.");
            carHomingLogged = false; // reset flag
        }
    } else {
        if (!isCarMoving) controlCarMovement(DIR_DOWN);
        if (!carHomingLogged) {
            LOG_I("FSM: Car Homing in progress (Moving DOWN).");
            carHomingLogged = true;
        }
    }
}

void handleReadyAuto() {
    // 1. Sinkronisasi State jika pergerakan fisik berhenti
    if ((currentCarState == CAR_MOVING_UP || currentCarState == CAR_MOVING_DOWN) && !isCarMoving) {
        currentCarState = CAR_IDLE;
    }

    // 2. Handling State Menunggu Pintu Terbuka
    if (currentCarState == CAR_WAITING_DOOR_OPEN) {
        if (currentDoorState != DOOR_OPEN && currentDoorState != DOOR_MOVING_TO_OPEN) {
            controlDoor(true);
        }
        return;
    }

    // 3. Logika Utama Pengambilan Keputusan (IDLE atau MOVING)
    if (currentCarState == CAR_IDLE) {
        controlCarMovement(DIR_IDLE);

        // Ambil target terbaik berdasarkan Scoring (Aging + Distance + LOOK)
        int nextTarget = getNextTargetFromQueue();
        
        if (nextTarget != 0) {
            targetFloor = nextTarget;
            // NOTE: Jangan pop (clear bit) di sini! 
            // Clear bit hanya dilakukan saat lift BENAR-BENAR sampai di lantai tujuan.
            // Agar jika lift terhenti di tengah jalan, target tidak hilang dari memori.
        }

        // Eksekusi Pergerakan jika target sudah ditentukan
        if (targetFloor != currentFloor && targetFloor != 0) {
            if (currentDoorState == DOOR_CLOSED) {
                Direction nextDir = (targetFloor > currentFloor) ? DIR_UP : DIR_DOWN;
                controlCarMovement(nextDir);
                
                if (currentDirection != DIR_IDLE) {
                    currentCarState = (nextDir == DIR_UP) ? CAR_MOVING_UP : CAR_MOVING_DOWN;
                    LOG_I("FSM: Moving to F%d (%s). Score-based decision.", targetFloor,
                          nextDir == DIR_UP ? "UP" : "DOWN");
                }
                fsmClosingDoorLogged = false;
            } else {
                // Pastikan pintu tertutup sebelum mulai bergerak
                controlDoor(false);
                if (!fsmClosingDoorLogged) {
                    LOG_I("FSM: Closing door before moving to F%d", targetFloor);
                    fsmClosingDoorLogged = true;
                }
            }
        }
    } 
    else if (currentCarState == CAR_MOVING_UP || currentCarState == CAR_MOVING_DOWN) {
        // --- OPPORTUNISTIC EVALUATION ---
        // Saat sedang meluncur, terus cek antrean. 
        // Jika ada panggilan baru yang "searah" dan "lebih dekat" (LOOK Logic),
        // getNextTargetFromQueue() akan mengembalikan lantai tersebut sebagai targetFloor baru.
        int dynamicTarget = getNextTargetFromQueue();
        if (dynamicTarget != 0 && dynamicTarget != targetFloor) {
            // Update target secara dinamis jika ditemukan lantai yang harus disinggahi
            targetFloor = dynamicTarget;
        }
    }
}

// ==== Main FSM Handler ====
void handleAutoModeFSM() {
    if (currentMode != MODE_AUTO) {
        controlCarMovement(DIR_IDLE);
        currentSystemState = SYS_SAFE_STATE;
        currentCarState = CAR_IDLE;
        last_fsm_action_logged = FSM_IDLE;
        return;
    }

    // Timeout proteksi
    if (isCarMoving && (millis() - carMoveStartTime > CAR_MAX_MOVE_DURATION_MS)) {
        controlCarMovement(DIR_IDLE);
        criticalError = true;
        currentCarState = CAR_ERROR_TIMEOUT;
        LOG_E("CAR ERROR: Stuck between floors! Timeout exceeded.");
        return;
    }

    // Safety check
    if (!safetyOK || criticalError) {
        controlCarMovement(DIR_IDLE);
        currentSystemState = SYS_SAFE_STATE;
        currentCarState = CAR_IDLE;
        return;
    }

    // Auto-recovery
    if (currentCarState == CAR_ERROR_TIMEOUT && !criticalError) {
        currentCarState = CAR_IDLE;
        LOG_I("CAR: Auto-recovery from timeout. State -> IDLE.");
    }

    SystemState prevState = currentSystemState;

    switch (currentSystemState) {
        case SYS_SAFE_STATE:       handleSafeState(); break;
        case SYS_INIT_DOOR_HOMING: handleDoorHoming(); break;
        case SYS_INIT_CAR_HOMING:  handleCarHoming(); break;
        case SYS_READY_AUTO:       handleReadyAuto(); break;
    }

    if (prevState != currentSystemState) {
        LOG_I("SYSTEM STATE CHANGE: %s -> %s", systemStateToString(prevState), systemStateToString(currentSystemState));
    }
}

// ==========================================================
// === FUNGSI SENSOR LIMIT LANTAI (REVISI LOGIKA ARRIVAL) ===
// ==========================================================

void checkFloorLimitSensors() {
    for (int i = 1; i <= TOTAL_FLOORS; i++) { 
        bool flag_status = false;
        unsigned long triggerTime = 0;

        // --- Ambil flag ISR secara atomic ---
        noInterrupts();
        flag_status  = floorIsrFlag[i];
        triggerTime  = floorIsrTriggerTime[i];
        interrupts();

        if (flag_status) {
            // --- Debounce: bypass jika simulator (triggerTime == 0) ---
            if (triggerTime == 0 || (millis() - triggerTime) > DEBOUNCE_TIME_MS) {
                
                // --- Validasi sensor aktif ---
                if (digitalRead(CAR_LS_PINS[i]) == LOW || flag_status) {
                    if (!lastLimitStatus[i]) {
                        lastLimitStatus[i] = true;
                        currentFloor = i;

                        // --- Safety: reset timer jika sedang bergerak ---
                        if (isCarMoving) {
                            carMoveStartTime = millis();
                            LOG_D("SAFETY: Timer reset at F%d. Target: F%d", i, targetFloor);
                        }

                        LOG_D("LS%d Hit! Position: F%d | Target: F%d", i, currentFloor, targetFloor);

                        // --- Logika Homing ---
                        if (!isCarPositionKnown) {
                            if (currentFloor == 1) {
                                controlCarMovement(DIR_IDLE);
                                isCarPositionKnown = true;
                                targetFloor = 1;
                                LOG_I("HOMING OK: Fixed at F1");
                            }
                        }
                        // --- Logika Normal ---
                        else {
    if (currentMode == MODE_INSPECTION) {
        controlCarMovement(DIR_IDLE);
    } else if (currentMode == MODE_AUTO) {
       if (isCarMoving && targetFloor == currentFloor) {
    controlCarMovement(DIR_IDLE);
    currentCarState = CAR_WAITING_DOOR_OPEN;

    // --- SINKRONISASI QUEUE & AGING ---
    pending_calls &= ~(1 << currentFloor); // Hapus dari bitmask
    callAge[currentFloor] = 0;             // Reset umur panggilan (PENTING!)
    targetFloor = 0;                       // Siapkan untuk pencarian target berikutnya

    triggerArrivalBeep();
    LOG_I("ARRIVED: F%d. Queue & Aging reset.", currentFloor);
                                }
                            }
                        }
                    }
                }

                // --- Reset flag setelah diproses ---
                noInterrupts();
                floorIsrFlag[i] = false;
                interrupts();
            }
        } else {
            // --- Logika rilis sensor ---
            if (digitalRead(CAR_LS_PINS[i]) == HIGH && lastLimitStatus[i]) {
                lastLimitStatus[i] = false;
            }
        }
    }
}

// ===============================================
// === FUNGSI HANDLE SYSTEM MODE CHANGE (DENGAN DEBOUNCE) ===
// ===============================================

void handleSystemModeChange() {
    unsigned long now = millis();
    if (now - lastModeReadTime < MODE_DEBOUNCE_MS) return;

    bool autoSel = (digitalRead(MODE_AUTO_SEL) == LOW);
    bool inspSel = (digitalRead(MODE_INSP_SEL) == LOW);

    Mode newMode;
    if (inspSel && !autoSel) {
        newMode = MODE_INSPECTION;
    } else if (autoSel && !inspSel) {
        newMode = MODE_AUTO;
    } else {
        newMode = MODE_OFF;
    }

    if (newMode != currentMode) {
        LOG_I("MODE CHANGE: %s -> %s", modeToString(currentMode), modeToString(newMode));
        currentMode = newMode;

        // Stop semua gerakan
        controlCarMovement(DIR_IDLE);
        stopDoorRelay();

        doorMoveStartTime = 0;
        doorDwellStartTime = 0;
        isDoorJogOpenHeld = false;
        isDoorJogCloseHeld = false;
        last_fsm_action_logged = FSM_IDLE;

        if (currentMode == MODE_AUTO) {
            if (isCarPositionKnown && currentDoorState == DOOR_CLOSED) {
                currentSystemState = SYS_READY_AUTO;
            } else {
                currentSystemState = SYS_INIT_DOOR_HOMING;
            }
            currentCarState = CAR_IDLE;
        } else {
            currentSystemState = SYS_SAFE_STATE;
            currentCarState = CAR_IDLE;
        }

        if (isCarPositionKnown) {
            targetFloor = currentFloor;
        } else {
            targetFloor = 1;
        }
    }

    lastModeReadTime = now;
}

void handleCriticalErrorState() {
    safetyOK = (digitalRead(SAFETY_CHAIN_IN) == LOW); 

    if (!safetyOK && !safetyFaultLogged) {
        LOG_E("SAFETY CHAIN BROKEN!");
        safetyFaultLogged = true;
        buzzerSoftFaultEndTime = millis() + SOFT_FAULT_BEEP_DURATION_MS * 3; 
    }
    if (!safetyOK) {
        digitalWrite(RELAY_CAR_UP, LOW); digitalWrite(RELAY_CAR_DOWN, LOW);
        stopDoorRelay(); 
        currentSystemState = SYS_SAFE_STATE; 
    } else {
        safetyFaultLogged = false; 
    }

    if (criticalError && !criticalErrorLogged) {
        LOG_E("CRITICAL ERROR ACTIVE!");
        criticalErrorLogged = true;
        buzzerSoftFaultEndTime = millis() + SOFT_FAULT_BEEP_DURATION_MS * 5; 
    }
    if (criticalError) {
        digitalWrite(RELAY_CAR_UP, LOW); digitalWrite(RELAY_CAR_DOWN, LOW);
        stopDoorRelay(); 
        currentSystemState = SYS_SAFE_STATE; 
    } else {
        criticalErrorLogged = false; 
    }
}

void handleHardFaultReset() {
    bool isJogUp = (digitalRead(MANUAL_CAR_JOG_UP_PIN) == LOW);
    bool isJogDown = (digitalRead(MANUAL_CAR_JOG_DOWN_PIN) == LOW);

    // Deteksi penekanan tombol JOG UP dan JOG DOWN secara bersamaan
    if (isJogUp && isJogDown) {
        if (hardFaultResetHoldStartTime == 0) {
            hardFaultResetHoldStartTime = millis(); 
            LOG_D("RESET: Hold start detected.");
        }
        if (millis() - hardFaultResetHoldStartTime >= HARD_FAULT_RESET_HOLD_MS) {
            if (criticalError) {
                // Lakukan reset hanya jika ada error kritis
                criticalError = false;
                criticalErrorLogged = false;
                safetyFaultLogged = false; 
                hardFaultResetHoldStartTime = 0; 
                buzzerSoftFaultEndTime = millis() + 500; 
                LOG_I("HARD FAULT RESET: Tombol Inspeksi ditahan %lu ms. Critical Error cleared.", HARD_FAULT_RESET_HOLD_MS);
            }
        }
    } else {
        hardFaultResetHoldStartTime = 0;
    }
}

// =============================================================================
// === 13. BUZZER, DISPLAY & INDICATOR MANAGER ===
// =============================================================================

// Trigger arrival beep (double beep pendek)
void triggerArrivalBeep() {
    arrivalBeep.count = 4;      // on-off-on-off → 2 beep
    arrivalBeep.next = millis();
    arrivalBeep.state = false;
}

// Service non-blocking arrival beep
void serviceArrivalBeep() {
    if (arrivalBeep.count == 0) return;

    unsigned long now = millis();
    if (now >= arrivalBeep.next) {
        arrivalBeep.state = !arrivalBeep.state;
        digitalWrite(BUZZER_PIN, arrivalBeep.state ? HIGH : LOW);
        arrivalBeep.next = now + 100;  // 100ms on/off
        if (!arrivalBeep.state) arrivalBeep.count--;
    }
}

// Update semua indikator: buzzer prioritas, heartbeat, lampu kabin
void updateIndicators() {
    unsigned long currentTime = millis();
    bool buzzerOn = false;

    // Prioritas 1: Safety chain putus → beep CEPAT (200ms on/off)
    if (!safetyOK) {
        buzzerOn = (currentTime % 400 < 200);
    }
    // Prioritas 2: Critical error → beep LAMBAT (1s on / 1s off)
    else if (criticalError) {
        buzzerOn = (currentTime % 2000 < 1000);
    }
    // Prioritas 3: Soft fault sementara
    else if (currentTime < buzzerSoftFaultEndTime) {
        buzzerOn = (currentTime % 600 < 300);
    }
    // Prioritas 4: Arrival beep (non-blocking, otomatis override jika aktif)

    // Heartbeat LED (selalu aktif, bahkan saat error → diagnosa MCU hidup)
    static unsigned long lastHeartbeatToggle = 0;
    if (currentTime - lastHeartbeatToggle >= HEARTBEAT_INTERVAL_MS) {
        digitalWrite(LED_HEARTBEAT, !digitalRead(LED_HEARTBEAT));
        lastHeartbeatToggle = currentTime;
    }

    // Output akhir buzzer & lampu kabin
    digitalWrite(BUZZER_PIN, buzzerOn);
    digitalWrite(LIGHT_CABIN_PIN, isLightOn);
}

void updateFaultCounter(const char* code) {
    if (String(code) != lastFaultCode) {
        faultStartTime = millis();
        lastFaultCode = code;
        totalFaultCount++;   // increment counter, bukan criticalError
        criticalError = true; // tandai bahwa fault sedang aktif
    }
}

void displayCriticalFault() {
    // ambil status fault
    const char* code = faultCodeToString();
    const char* desc = faultDescription(code);

    // update counter
    updateFaultCounter(code);

    // clear hanya bagian yang perlu (opsional: bisa pakai fillRect untuk hemat waktu)
    display.clearDisplay();

    // header
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(2, 0);
    display.printf("ORCH_%s | Flt:%lu", FIRMWARE_VERSION, totalFaultCount);
    display.drawLine(0, 10, 128, 10, SSD1306_WHITE);

    // judul besar
    display.setTextSize(2);
    display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
    uint16_t title_x = (128 - (strlen(code) * (2 * 6))) / 2;
    display.setCursor(title_x, 14);
    display.print(code);

    // deskripsi
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    uint16_t line_x = (128 - (strlen(desc) * 6)) / 2;
    display.setCursor(line_x, 35);
    display.print(desc);

    // baris bawah: state + durasi
    display.drawLine(0, 53, 128, 53, SSD1306_WHITE);
    display.setCursor(2, 45);
    display.printf("State:%s", systemStateToString(currentSystemState));

    unsigned long faultDuration = (millis() - faultStartTime) / 1000;
    display.setCursor(90, 57);
    display.printf("%lus", faultDuration);

    // kirim buffer ke OLED
    display.display();
}

void updateOLED() {
    static bool lossAlreadyCounted = false;

     if (criticalError) {
     displayCriticalFault();
     return;
     }
     
    display.clearDisplay();
    display.fillRect(0, 0, 128, 12, SSD1306_WHITE); 
    display.setTextColor(SSD1306_BLACK);
    display.setTextSize(1);
    display.setCursor(2, 2);
    display.print(modeToString(currentMode)); 
    display.setCursor(70, 2); 
    display.print("F");
      if (currentSystemState == SYS_INIT_CAR_HOMING) {
    // Tampilkan animasi atau huruf 'H' saat sedang mencari lantai dasar
    static uint8_t animFrame = 0;
    const char* animChars[] = {"-", "\\", "|", "/"};
    display.print(animChars[animFrame % 4]);
    animFrame++;
    display.print(" Homing");
} 
else if (isCarPositionKnown) {
    // Jika posisi sudah dikenal, tampilkan angka lantai terakhir
    display.print(currentFloor);
    
    if (isCarMoving) {
        display.print(currentDirection == DIR_UP ? " \x18" : " \x19"); // Simbol panah
    }
} 
else {
    // Jika sistem baru nyala dan belum homing
    display.print("H");
}

    display.print(" -> F"); display.print(targetFloor); 

    display.setTextColor(SSD1306_WHITE);
    display.drawLine(0, 13, 128, 13, SSD1306_WHITE);

    int y = 16;
    display.setCursor(0, y);
    display.print("CABIN : ");
    if (currentSystemState == SYS_READY_AUTO) display.print(carStateToString(currentCarState));
    else if (isCarMoving) display.print(currentDirection == DIR_UP ? "\x18 MOVE" : "\x19 MOVE");
    else display.print(systemStateToString(currentSystemState));

    if (isCarMoving && currentSystemState != SYS_INIT_CAR_HOMING) {
        display.print(" "); display.print((millis() - carMoveStartTime) / 1000); display.print("s");
    }
    y += 9;

    display.setCursor(0, y);
    display.print("DOOR : "); display.print(doorStateToString(currentDoorState));

    if (currentDoorState == DOOR_MOVING_TO_OPEN || currentDoorState == DOOR_MOVING_TO_CLOSE || currentDoorState == DOOR_HOMING_TO_CLOSE || currentDoorState == DOOR_MANUAL_JOG) 
    {
        display.print(" (");
switch (currentDoorState) {
    case DOOR_OPEN:           display.print("OPEN");    break;
    case DOOR_CLOSED:         display.print("CLOSED");  break;
    case DOOR_MOVING_TO_OPEN: display.print(">>"); break;  // panah buka
    case DOOR_MOVING_TO_CLOSE:display.print("<<");break;  // panah tutup
    case DOOR_STOPPED_MID_MOVE: display.print("MID");   break;
    case DOOR_ERROR:          display.print("ERR");     break;
    case DOOR_MANUAL_JOG:     display.print("JOG");     break;
    default:                  display.print("?");       break;
}
display.print(")");
    }
    y += 9;

    display.setCursor(0, y);
    display.print("SAFE : ");
    display.print(safetyOK ? "OK" : "FAULT"); display.print(" ERR:"); display.print(criticalError ? "YES" : "NO");
    y += 9;

    display.drawLine(0, y - 1, 128, y - 1, SSD1306_WHITE); 

    display.setCursor(0, y + 3);
    if (currentMode == MODE_INSPECTION) {
        display.print("JOG K:"); display.print(isJogUpHeld ? "UP " : "- "); display.print(isJogDownHeld ? "DN" : "-");
        display.print(" | P:"); display.print(isDoorJogOpenHeld ? "OP " : "- "); display.print(isDoorJogCloseHeld ? "CL" : "-");
        if (hardFaultResetHoldStartTime != 0) {
            display.setCursor(90, y + 3); display.print("RST_HOLD");
        }
    } else {

// --- Tampilan Status RS485 ---
display.setCursor(0, y + 3);

// Gunakan variabel commLost yang sudah diupdate oleh handleRS485Master
if (commLost) {
    // Tampilan Invert (Blink) untuk indikasi Error
    display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
    display.print("RS485 LOSS");
    display.setTextColor(SSD1306_WHITE);
} else {
    display.print("RS485 OK");
    // Indikator "Heartbeat" berkedip setiap paket diterima
    display.print(totalPacketsReceived % 2 == 0 ? " *" : "  ");
}

// --- Tampilan Counter di Sisi Kanan ---
display.setCursor(85, y + 3);
// Menampilkan jumlah total kejadian Loss sejak Master menyala
display.printf("Err:%lu", rs485LossCount);
    }

    display.setCursor(50, y + 13);
    display.print(FIRMWARE_VERSION);

    display.display();
}

// ==========================================================
// === FUNGSI RS-485 ===
// ==========================================================
void rs485MasterInit() {
    pinMode(RS485_DE_RE, OUTPUT);
    digitalWrite(RS485_DE_RE, LOW);
    pinMode(LIGHT_CABIN_PIN, OUTPUT);
    digitalWrite(LIGHT_CABIN_PIN, LOW);

    Serial2.begin(BAUDRATE, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN);

    currentCommState = COMM_IDLE;
    commLost = false;
    lastCommUpdate = millis();

    LOG_I("RS485: Master init @%lu baud", BAUDRATE);
}

// ==========================================================
// SEND PACKET
// ==========================================================
void sendRS485Packet(const uint8_t* data, size_t length) {
    // 1. Bersihkan sisa RX agar buffer bersih
    while (Serial2.available()) (void)Serial2.read();

    // 2. Transmit
    digitalWrite(RS485_DE_RE, HIGH);
    delayMicroseconds(10); // Stabilisasi jalur
    Serial2.write(data, length);
    
    // 3. Pastikan data benar-benar terkirim sebelum pindah ke RX
    Serial2.flush(); 
    delayMicroseconds(600); // Guard time untuk byte terakhir
    digitalWrite(RS485_DE_RE, LOW); 

    // 4. Update state tracking
    totalPacketsSent++;
    currentCommState = COMM_WAIT_RESPONSE;
    waitStartMs = millis();
    rxIndex = 0;
}

// ==========================================================
// PROCESS INCOMING COMMAND
// ==========================================================
void processCommand(uint8_t* packet) {
    // Validasi awal
    if (packet[PKT_IDX_START] != START_BYTE) {
        commStats.consecutiveCommErrors++;
        return;
    }

    // Master harus menerima paket yang ditujukan ke dirinya (MASTER_ADDRESS)
    if (packet[PKT_IDX_ADDR] != MASTER_ADDRESS) {
        commStats.consecutiveCommErrors++;
        return;
    }

    // CRC check
    uint16_t received_crc   = (uint16_t)packet[PKT_IDX_CRC_LO] | ((uint16_t)packet[PKT_IDX_CRC_HI] << 8);
    uint16_t calculated_crc = calculateCRC(packet, PACKET_SIZE - 2); 
    if (received_crc != calculated_crc) {
        commStats.consecutiveCommErrors++;
        totalCrcErrors++;
        return;
    }

    // Reset error counter jika sukses
    commStats.consecutiveCommErrors = 0;
    commLost                        = false;
    commStats.lastSuccessfulCommMs  = millis();
    lastSuccessfulCommMs            = commStats.lastSuccessfulCommMs;
    commStats.totalPacketsReceived++;
    totalPacketsReceived++;

    // Proses command
    uint8_t cmd = packet[PKT_IDX_CMD]; 
    uint8_t val = packet[PKT_IDX_FLOOR]; 

    if (cmd != CMD_JOG_DOOR_OPEN && cmd != CMD_JOG_DOOR_CLOSE) {
        isDoorJogOpenHeld  = false;
        isDoorJogCloseHeld = false;
    }

    switch (cmd) {
        case CMD_CAR_CALL: 
            if (currentMode == MODE_AUTO && !criticalError && val >= 1 && val <= TOTAL_FLOORS) {
            addCallToQueue(val);   // gunakan queue agar immediate open aktif
            LOG_I("RS485: Car call received for F%d", val);
            }
            break;

        case CMD_DOOR_OPEN: 
            if (!isCarMoving) controlDoor(true);
            break;

        case CMD_DOOR_CLOSE: 
            if (!isCarMoving) controlDoor(false);
            break;

        case CMD_JOG_DOOR_OPEN:
            if (currentMode == MODE_INSPECTION) {
                isDoorJogOpenHeld  = true;
                isDoorJogCloseHeld = false;
                lastJogReceivedMs  = millis();
            }
            break;

        case CMD_JOG_DOOR_CLOSE:
            if (currentMode == MODE_INSPECTION) {
                isDoorJogCloseHeld = true;
                isDoorJogOpenHeld  = false;
                lastJogReceivedMs  = millis();
            }
            break;

        case CMD_EMERGENCY:
            criticalError        = true;
            controlCarMovement(DIR_IDLE);
            stopDoorRelay();
            currentSystemState   = SYS_SAFE_STATE;
            currentCarState      = CAR_IDLE;
            currentDoorState     = DOOR_ERROR;
            buzzerSoftFaultEndTime = millis() + SOFT_FAULT_BEEP_DURATION_MS * 10;
            LOG_E("EMERGENCY: Triggered from COP via RS485!");
            break;

        case CMD_LIGHT_TOGGLE:
            isLightOn = !isLightOn;
            digitalWrite(LIGHT_CABIN_PIN, isLightOn);
            LOG_I("LIGHT: Toggled from COP → %s", isLightOn ? "ON" : "OFF");
            break;

        case ACK:
            break;
    }
}

// ==========================================================
// HANDLE MASTER LOOP
// ==========================================================
void handleRS485Master() {
    // 1. LOGIKA RECOVERY
    if (commLost && (millis() - lastRecoveryAttemptMs >= 5000)) {
        lastRecoveryAttemptMs = millis();
        Serial2.end();
        Serial2.begin(BAUDRATE, SERIAL_8N1, RS485_RX_PIN, RS485_TX_PIN);
        currentCommState = COMM_IDLE;
        rxIndex = 0;
    }

    // 2. POLLING INTERVAL
    if (currentCommState == COMM_IDLE && (millis() - lastCommUpdate) >= COMM_UPDATE_INTERVAL_MS) {
        lastCommUpdate = millis();

        // Persiapkan paket
        uint8_t pollPkt[PACKET_SIZE];
        memset(pollPkt, 0, sizeof(pollPkt)); // Pastikan memory bersih
        
        pollPkt[PKT_IDX_START]     = START_BYTE;
        pollPkt[PKT_IDX_ADDR]      = SLAVE_ADDRESS;
        pollPkt[PKT_IDX_CMD]       = CMD_POLL_STATUS;
        pollPkt[PKT_IDX_FLOOR]     = currentFloor;
        pollPkt[PKT_IDX_MODE]      = (uint8_t)currentMode;
        pollPkt[PKT_IDX_DIR_FAULT] = (uint8_t)currentDirection | (criticalError ? 0x80 : 0x00);

        uint16_t crc = calculateCRC(pollPkt, PACKET_SIZE - 2);
        pollPkt[PKT_IDX_CRC_LO] = crc & 0xFF;
        pollPkt[PKT_IDX_CRC_HI] = (crc >> 8) & 0xFF;

        // Reset parser RX & kuras buffer hardware sebelum kirim baru
        rxIndex = 0; 
        while(Serial2.available()) Serial2.read(); 
        
        // Kirim paket
        sendRS485Packet(pollPkt, PACKET_SIZE); 
    }

    // 3. FSM TUNGGU RESPON
    if (currentCommState == COMM_WAIT_RESPONSE) {
        while (Serial2.available()) {
            uint8_t b = Serial2.read();
            
            if (rxIndex == 0) {
                if (b == START_BYTE) rxBuffer[rxIndex++] = b;
            } else {
                rxBuffer[rxIndex++] = b;
                if (rxIndex >= PACKET_SIZE) {
                    processCommand(rxBuffer);
                    currentCommState = COMM_IDLE; 
                    rxIndex = 0;
                }
            }
        }

        // Handle Timeout
        if ((millis() - waitStartMs) > COMM_TIMEOUT_MS) {
            commStats.consecutiveCommErrors++;
            currentCommState = COMM_IDLE; 
            rxIndex = 0;
            
            if (commStats.consecutiveCommErrors >= MAX_CONSECUTIVE_ERRORS && !commLost) {
                commLost = true;
                LOG_E("RS485: Connection Lost - Slave Unresponsive");
            }
        }
    }
}

// =============================================================================
// === MCC ORCH SS OPTIMIZED - INDUSTRIAL TEST HARNESS ===
// =============================================================================
#ifdef ENABLE_DEBUG
void runSafetyPlausibility() {
    static int lastTrackedFloor = -1;
    unsigned long now = millis();

    if (isCarMoving && currentMode == MODE_AUTO) {
        if (now - carMoveStartTime > CAR_MAX_MOVE_DURATION_MS) {
            LOG_E("STRESS_TEST: Watchdog Triggered! Car stuck for %lu ms", CAR_MAX_MOVE_DURATION_MS);
            criticalError = true; 
        }
    }

    if (currentFloor != lastTrackedFloor && lastTrackedFloor != -1 && isCarMoving) {
        bool illegalUp = (currentDirection == DIR_UP && currentFloor < lastTrackedFloor);
        bool illegalDown = (currentDirection == DIR_DOWN && currentFloor > lastTrackedFloor);
        if (illegalUp || illegalDown) {
            LOG_E("SAFETY_VIOLATION: Direction mismatch! F%d to F%d while moving %s", 
                  lastTrackedFloor, currentFloor, (currentDirection == DIR_UP ? "UP" : "DOWN"));
            criticalError = true;
        }
        lastTrackedFloor = currentFloor;
    }
}

void printHelp() {
    Serial.println(F("\n=== MCC ORCH SS OPTIMIZED - SIMULATOR COMMANDS ==="));
    Serial.println(F("call [f]      - Kirim panggilan ke lantai f (Force Bypass)"));
    Serial.println(F("hit [f]       - Simulasi sensor lantai f (Virtual Hit)"));
    Serial.println(F("diag          - Laporan status FSM, Safety, & Comm"));
    Serial.println(F("emergency     - Paksa Emergency Stop"));
    Serial.println(F("reset         - Clear Critical Error & Force Ready"));
    Serial.println(F("open/close    - Buka/Tutup pintu paksa"));
    Serial.println(F("help          - Tampilkan menu ini"));
    Serial.println(F("====================================================\n"));
}

void handleSerialMonitor() {
    if (!Serial.available()) return;
    
    String cmd = Serial.readStringUntil('\n');
    cmd.trim(); cmd.toLowerCase();

    if (cmd == "help") {
        printHelp();
    }
else if (cmd.startsWith("call ")) {
    int f = cmd.substring(5).toInt();
    if (f >= 1 && f <= TOTAL_FLOORS && !criticalError) {
        // Bersihkan flag hit dan lastLimitStatus (simulasi call tidak boleh mengganggu ISR)
        noInterrupts();
        for(int i = 1; i <= TOTAL_FLOORS; i++) {
            floorIsrFlag[i] = false;
            lastLimitStatus[i] = false;
        }
        interrupts();

        if (!isCarPositionKnown) isCarPositionKnown = true; // untuk test harness
        addCallToQueue(f); // <-- gunakan antrean, bukan set targetFloor langsung
        LOG_I("SIMULATOR: New Call F%d. Added to queue.", f);
    }
}
    else if (cmd.startsWith("hit ")) {
        int f = cmd.substring(4).toInt();
        if (f >= 1 && f <= TOTAL_FLOORS) {
            LOG_I("SIMULATOR: Pushing virtual sensor hit F%d", f);
            noInterrupts();
            floorIsrFlag[f] = true;
            floorIsrTriggerTime[f] = 0; 
            interrupts();
        }
    }
    else if (cmd == "diag") {
        Serial.println(F("\n--- FSM DIAGNOSTIC REPORT ---"));
        Serial.printf("Mode      : %s\n", modeToString(currentMode));
        Serial.printf("System    : %s\n", systemStateToString(currentSystemState));
        Serial.printf("Car FSM   : %s | Door FSM: %s\n", carStateToString(currentCarState), doorStateToString(currentDoorState));
        Serial.printf("Position  : F%d -> Target F%d (Known: %s)\n", currentFloor, targetFloor, isCarPositionKnown?"YES":"NO");
        Serial.printf("Safety    : Chain=%s | Critical=%s\n", safetyOK?"OK":"FAIL", criticalError?"YES":"NO");
        Serial.printf("RS485     : Sent=%lu Recv=%lu CRCerr=%lu Retries=%lu\n", 
            (unsigned long)totalPacketsSent, (unsigned long)totalPacketsReceived, 
            (unsigned long)totalCrcErrors, (unsigned long)totalRetries);
        Serial.println(F("-----------------------------\n"));
    }
    else if (cmd == "open") {
    if (!criticalError) {
        if (currentMode != MODE_INSPECTION && isCarMoving) {
            LOG_W("SERIAL: OPEN ignored, car is moving.");
        } else {
            controlDoor(true);
            LOG_I("SERIAL: Manual OPEN command executed.");
        }
    } else {
        LOG_E("SERIAL: Cannot OPEN, critical error active.");
    }
}
else if (cmd == "close") {
    if (!criticalError) {
        controlDoor(false);
        LOG_I("SERIAL: Manual CLOSE command executed.");
    } else {
        LOG_E("SERIAL: Cannot CLOSE, critical error active.");
    }
}
else if (cmd == "emergency") {
    criticalError = true;
    controlCarMovement(DIR_IDLE);
    stopDoorRelay();
    currentSystemState = SYS_SAFE_STATE;
    currentCarState = CAR_IDLE;
    currentDoorState = DOOR_ERROR;
    buzzerSoftFaultEndTime = millis() + SOFT_FAULT_BEEP_DURATION_MS * 10;
    LOG_E("SERIAL: EMERGENCY STOP triggered via serial command!");
}
else if (cmd == "reset") {
    criticalError = false;
    criticalErrorLogged = false;
    currentCarState = CAR_IDLE;
    currentSystemState = SYS_READY_AUTO;
    LOG_I("SERIAL: System RESET performed via serial command.");
}
#endif
}

// ===============================================
// === 10. SETUP & LOOP (FUNGSI UTAMA) ===
// ===============================================

void setup() {
    Serial.begin(115200);
    delay(100);
    LOG_I("--- Starting %s ---", FIRMWARE_VERSION);

    // OLED init
    Wire.begin();
    if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
        LOG_E("OLED failed to initialize!");
    }
    display.clearDisplay();
    display.display();
    delay(500);

    rs485MasterInit();

    // Aktuator pins
    pinMode(RELAY_CAR_UP, OUTPUT);
    pinMode(RELAY_CAR_DOWN, OUTPUT);
    digitalWrite(RELAY_CAR_UP, LOW);
    digitalWrite(RELAY_CAR_DOWN, LOW);

    pinMode(RELAY_OPEN, OUTPUT);
    pinMode(RELAY_CLOSE, OUTPUT);
    digitalWrite(RELAY_OPEN, LOW);
    digitalWrite(RELAY_CLOSE, LOW);

    pinMode(LED_HEARTBEAT, OUTPUT);
    pinMode(BUZZER_PIN, OUTPUT);
    digitalWrite(BUZZER_PIN, LOW);

    pinMode(LIGHT_CABIN_PIN, OUTPUT);

    // Input pins
    pinMode(DOOR_SENSOR_CLOSED, INPUT_PULLUP);
    pinMode(DOOR_SENSOR_OPEN, INPUT_PULLUP);

    pinMode(CAR_LS_FLOOR_1, INPUT_PULLUP);
    pinMode(CAR_LS_FLOOR_2, INPUT_PULLUP);
    pinMode(CAR_LS_FLOOR_3, INPUT);

    pinMode(MANUAL_CAR_JOG_UP_PIN, INPUT_PULLUP);
    pinMode(MANUAL_CAR_JOG_DOWN_PIN, INPUT_PULLUP);

    pinMode(SAFETY_CHAIN_IN, INPUT);
    pinMode(MODE_AUTO_SEL, INPUT);
    pinMode(MODE_INSP_SEL, INPUT);

    // ISR attach
    attachInterrupt(digitalPinToInterrupt(CAR_LS_FLOOR_1), isr_floor_1, FALLING);
    attachInterrupt(digitalPinToInterrupt(CAR_LS_FLOOR_2), isr_floor_2, FALLING);
    attachInterrupt(digitalPinToInterrupt(CAR_LS_FLOOR_3), isr_floor_3, FALLING);
    LOG_I("ISR attached to F1, F2, F3.");

    // RELAY DOOR
    doorInit();
    doorInitializePosition();

    bool ls_open  = isOpenLimitActive();
    bool ls_close = isCloseLimitActive();

    // ── Kasus normal: salah satu sensor aktif ───────────────────────────────
    if (ls_close) {
        currentDoorState = DOOR_CLOSED;
        doorTotalMotionStart = 0;
        doorCloseRetryCount = doorOpenRetryCount = 0;
        LOG_I("DOOR INIT: Closed position detected (limit switch confirmed)");
        return;
    }

    if (ls_open) {
        currentDoorState = DOOR_OPEN;
        doorTotalMotionStart = 0;
        doorCloseRetryCount = doorOpenRetryCount = 0;
        // Mulai dwell timer jika mode auto (opsional, tergantung logic)
        if (doorDwellStartTime == 0) {
            doorDwellStartTime = millis();
        }
        LOG_I("DOOR INIT: Open position detected (limit switch confirmed)");
        return;
    }

    // ── Tidak ada sensor aktif (posisi tengah / unknown) ────────────────────
    currentDoorState = DOOR_STOPPED_MID_MOVE;
    doorTotalMotionStart = 0;
    LOG_W("DOOR INIT: No limit switch active → assumed mid-position");
    LOG_I("DOOR: Homing or manual command required to determine position");

    // Opsional: auto-homing ke close jika sistem tidak dalam MODE_OFF
    // (bisa diaktifkan jika ingin lebih otomatis)
    if (currentMode != MODE_OFF) {
       LOG_I("DOOR: Auto-homing to close initiated...");
      controlDoor(false);
 }

    carInit();

    handleSystemModeChange();
}

void loop() {
    // 2. Safety check
    safetyOK = (digitalRead(SAFETY_CHAIN_IN) == LOW);
    handleSystemModeChange();

    // 3. Jog debounce
    static unsigned long lastJogTime = 0;
    if (millis() - lastJogTime > DEBOUNCE_TIME_MS) {
        isJogUpHeld   = (digitalRead(MANUAL_CAR_JOG_UP_PIN) == LOW);
        isJogDownHeld = (digitalRead(MANUAL_CAR_JOG_DOWN_PIN) == LOW);
        lastJogTime   = millis();
    }

    // 4. Jog timeout (inspection mode)
    if (currentMode == MODE_INSPECTION && (isDoorJogOpenHeld || isDoorJogCloseHeld)) {
        if (millis() - lastJogReceivedMs > JOG_TIMEOUT_MS) {
            isDoorJogOpenHeld  = false;
            isDoorJogCloseHeld = false;
            LOG_I("INSPECTION: JOG timeout, reset held state");
        }
    }

    // 5. Hard fault reset
    handleHardFaultReset();

    // 6. RS485 comm
    handleRS485Master();

    // 7. Critical error state
    handleCriticalErrorState();

    // 8. Error block
    if (!safetyOK || criticalError) {
        controlCarMovement(DIR_IDLE);
        stopDoorRelay();
        maintenanceUpdate();
        return;
    }

    // 9. FSM updates
    checkFloorLimitSensors();

    #ifdef ENABLE_DEBUG
    handleSerialMonitor();
    runSafetyPlausibility();
    #endif

    handleDoorFSM();

    // 10. Main FSM
    switch (currentMode) {
        case MODE_OFF:
            currentSystemState = SYS_SAFE_STATE;
            controlCarMovement(DIR_IDLE);
            stopDoorRelay();
            break;

        case MODE_AUTO:
            handleAutoModeFSM();
            break;

        case MODE_INSPECTION: {
            currentSystemState = SYS_SAFE_STATE;
            Direction jogDir   = DIR_IDLE;

            if (isDoorJogOpenHeld != isDoorJogCloseHeld) {
                controlCarMovement(DIR_IDLE);
                if (isDoorJogOpenHeld) {
                    controlDoor(true);
                } else if (isDoorJogCloseHeld) {
                    controlDoor(false);
                }
            } else if (isJogUpHeld != isJogDownHeld) {
                jogDir = isJogUpHeld ? DIR_UP : DIR_DOWN;
                stopDoorRelay();
                controlCarMovement(jogDir);
            } else {
                stopDoorRelay();
                controlCarMovement(DIR_IDLE);
            }
            LOG_I("INSPECTION: Door jog %s", isDoorJogOpenHeld ? "OPEN" : (isDoorJogCloseHeld ? "CLOSE" : "STOP"));
            break; // ← tutup case MODE_INSPECTION dengan break
        }
    }
    // 11. UI & maintenance
    maintenanceUpdate();
}

// Helper untuk UI & maintenance
void maintenanceUpdate() {
    serviceArrivalBeep();
    updateIndicators();
    processLogBuffer();

        if (millis() - lastOledUpdate < OLED_UPDATE_INTERVAL_MS) return;
    lastOledUpdate = millis();
    updateOLED();
}