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

// =================================== OLED DISPLAY ===================================
const int SCREEN_WIDTH = 128;
const int SCREEN_HEIGHT = 64;
const int OLED_RESET = -1;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// =================================== GLOBAL VARIABLES ===================================
// --- Elevator State Variables ---
int totalFloors = 3;
int currentFloor = 0;
bool isEmergency = false;
bool cabinLightOn = false;

enum ElevatorState {
  IDLE, MOVING_UP, MOVING_DOWN, DOOR_OPENING, DOOR_CLOSING,
  EMERGENCY_STOP, EMERGENCY_RESETTING, CALIBRATING, CALIBRATION_FAILED, DOOR_OPEN
};

ElevatorState currentState = CALIBRATING;
ElevatorState lastDirection = MOVING_UP;

enum OperationMode {
  MODE_NORMAL, MODE_TEST, MODE_INSPECTION
};
OperationMode currentMode = MODE_NORMAL;

// --- Call and Queue Management ---
bool callUp[TOTAL_MAX_FLOORS] = {false};
bool callDown[TOTAL_MAX_FLOORS] = {false};
bool copCallActive[TOTAL_MAX_FLOORS] = {false};
bool lopCallActive[TOTAL_MAX_FLOORS] = {false};

// --- Input/Output Data (Shift Registers) ---
byte inputData[4];
byte outputData[2];

// --- Timers and Debounce Variables ---
unsigned long stateTimer = 0;
unsigned long motorStartTime = 0;
unsigned long lastActivityTime = 0;
unsigned long lastModeChangeTime = 0;
unsigned long lastSerialDataTime = 0;
unsigned long lastBlink = 0;
unsigned long previousMillis = 0;
unsigned long doorTimer = 0;
unsigned long lastOpenButtonTime = 0;
unsigned long lastCloseButtonTime = 0;

unsigned long lastButtonPressTime[TOTAL_MAX_FLOORS] = {0};
unsigned long lastLimitDebounceTime[TOTAL_MAX_FLOORS] = {0};

unsigned long lastInspUpTime = 0;
unsigned long lastInspDownTime = 0;
unsigned long lastCabinLightTime = 0;
unsigned long lastCallUpTime = 0;
unsigned long lastCallDownTime = 0;

bool lastTestModePinState = HIGH;
bool lastCabinLightButtonState = HIGH;
bool lastLimitStatus[TOTAL_MAX_FLOORS];
bool doorCommandSent = false;
bool blinkState = true;
int dotCount = 0;

// FINAL LIMIT & MOTOR CONTROL NON-BLOCKING
volatile bool finalLimitInterrupted = false;
volatile int interruptedPin = -1;
unsigned long lastFinalLimitHandleTime = 0;
const unsigned long FINAL_LIMIT_DEBOUNCE_MS = 200;

bool motorStopping = false;
unsigned long stopStartTime = 0;
const unsigned long STOP_DELAY_MS = 30;

bool waitingSwitch = false;
unsigned long switchStartTime = 0;
ElevatorState nextDirection = IDLE;

// =================================== FUNCTION PROTOTYPES ===================================
void readDipSwitch();
void controlMotor(ElevatorState state);
void stopMotor();
void displayStatus();
void displayCallQueue();
void processCallButtons();
void handleLimitSwitches();
void handleEmergency();
void calibrateLift();
void emergencyReset();
int findNextFloor();
void handleModeChange();
void handleNormalMode();
void handleTestMode();
void handleInspectionMode();
void drawArrowUp(int x, int y, int size);
void drawArrowDown(int x, int y, int size);
void readAllSRInputs();
bool getLimitSwitchStatus(int floor);
void writeAllSROutputs();
void updateFloorLeds();
void processDoorAndLightButtons();
void handleMotorTimeout();
void IRAM_ATTR handleFinalLimitUp();
void IRAM_ATTR handleFinalLimitDown();
void showStartup();
void handleDoorControl();
void updateMotorControl(); // Fungsi baru untuk logika non-blocking

// =================================== SETUP ===================================
void setup() {
  Serial.begin(115200);
  Serial.println("Orch_ Elevator Controller Starting...");

  pinMode(TEST_MODE_PIN, INPUT_PULLUP);
  pinMode(EMERGENCY_PIN, INPUT_PULLUP);
  pinMode(FINAL_LIMIT_UP_PIN, INPUT_PULLUP);
  pinMode(FINAL_LIMIT_DOWN_PIN, INPUT_PULLUP);

  // Attach interrupts
  attachInterrupt(digitalPinToInterrupt(FINAL_LIMIT_UP_PIN), handleFinalLimitUp, FALLING);
  attachInterrupt(digitalPinToInterrupt(FINAL_LIMIT_DOWN_PIN), handleFinalLimitDown, FALLING);

  pinMode(SR_IN_DATA, INPUT);
  pinMode(SR_IN_LATCH, OUTPUT);
  pinMode(SR_IN_CLOCK, OUTPUT);
  pinMode(SR_OUT_DATA, OUTPUT);
  pinMode(SR_OUT_LATCH, OUTPUT);
  pinMode(SR_OUT_CLOCK, OUTPUT);
  digitalWrite(SR_IN_LATCH, HIGH);
  digitalWrite(SR_OUT_LATCH, LOW);

  pinMode(RELAY_MOTOR_UP, OUTPUT);
  pinMode(RELAY_MOTOR_DOWN, OUTPUT);
  pinMode(RELAY_MOTOR_BRAKE, OUTPUT);
  stopMotor();

  Serial2.begin(9600, SERIAL_8N1, SERIAL_DOOR_RX, SERIAL_DOOR_TX);
  Serial1.begin(9600, SERIAL_8N1, -1, SERIAL_AUDIO_TX);

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

  pinMode(DIP_PIN_0, INPUT_PULLUP);
  pinMode(DIP_PIN_1, INPUT_PULLUP);
  pinMode(DIP_PIN_2, INPUT_PULLUP);
  pinMode(DIP_PIN_3, INPUT_PULLUP);
  readDipSwitch();

  readAllSRInputs();
  for (int i = 0; i < totalFloors; i++) {
    lastLimitStatus[i] = getLimitSwitchStatus(i + 1);
  }

  showStartup();
  calibrateLift();
  if (currentState != CALIBRATION_FAILED) {
    currentState = IDLE;
    Serial.println("Sistem siap. Status: IDLE");
  }
}

// =================================== MAIN LOOP ===================================
void loop() {
  // --- Final Limit handling (Non-blocking) ---
  if (finalLimitInterrupted) {
    unsigned long now = millis();
    if (now - lastFinalLimitHandleTime > FINAL_LIMIT_DEBOUNCE_MS) {
      Serial.print("Interupsi final limit terpicu di pin: ");
      Serial.println(interruptedPin);

      if (!isEmergency) {
        Serial.println("FINAL LIMIT SWITCH TERPICU! Lift dihentikan karena over-travel.");
        isEmergency = true;
        currentState = EMERGENCY_STOP;
      }

      stopMotor();
      lastFinalLimitHandleTime = now;
    }
    finalLimitInterrupted = false;
    interruptedPin = -1;
  }

  // Always read inputs, check for emergency, and handle mode changes
  readAllSRInputs();
  handleEmergency();
  handleModeChange();
  
  // Call non-blocking motor control logic every loop
  updateMotorControl(); 

  if (isEmergency || currentState == CALIBRATION_FAILED) {
    updateFloorLeds();
    displayStatus();
    return;
  }

  // Main state machine based on current mode
  switch (currentMode) {
    case MODE_NORMAL:
      handleNormalMode();
      break;
    case MODE_TEST:
      handleTestMode();
      break;
    case MODE_INSPECTION:
      handleInspectionMode();
      break;
    default:
      currentMode = MODE_NORMAL;
      break;
  }

  updateFloorLeds();
  displayStatus();
}

// =================================== ISRs (Interrupt Service Routines) ===================================
void IRAM_ATTR handleFinalLimitUp() {
  finalLimitInterrupted = true;
  interruptedPin = FINAL_LIMIT_UP_PIN;
}

void IRAM_ATTR handleFinalLimitDown() {
  finalLimitInterrupted = true;
  interruptedPin = FINAL_LIMIT_DOWN_PIN;
}

// =================================== OPERATIONAL MODE HANDLERS ===================================
void handleModeChange() {
  bool currentTestModePinState = digitalRead(TEST_MODE_PIN) == LOW;
  unsigned long now = millis();
  if (lastTestModePinState == HIGH && currentTestModePinState == LOW && (now - lastModeChangeTime) > DEBOUNCE_DELAY_MS) {
    switch (currentMode) {
      case MODE_NORMAL: currentMode = MODE_TEST; break;
      case MODE_TEST: currentMode = MODE_INSPECTION; break;
      case MODE_INSPECTION: currentMode = MODE_NORMAL; break;
    }
    Serial.print("Mode changed to: ");
    if (currentMode == MODE_NORMAL) Serial.println("NORMAL");
    else if (currentMode == MODE_TEST) Serial.println("TEST");
    else Serial.println("INSPECTION");
    lastModeChangeTime = now;
    stopMotor();
    currentState = IDLE;
    for (int i = 0; i < totalFloors; i++) {
      callUp[i] = callDown[i] = copCallActive[i] = lopCallActive[i] = false;
    }
  }
  lastTestModePinState = currentTestModePinState;
}

void handleNormalMode() {
  processCallButtons();
  processDoorAndLightButtons();
  handleLimitSwitches();
  handleMotorTimeout();

  static ElevatorState lastState = currentState;
  if (lastState != currentState) {
    Serial.print("State berubah menjadi: ");
    switch (currentState) {
      case IDLE: Serial.println("IDLE"); break;
      case MOVING_UP: Serial.println("MOVING_UP"); break;
      case MOVING_DOWN: Serial.println("MOVING_DOWN"); break;
      case DOOR_OPENING: Serial.println("DOOR_OPENING"); break;
      case DOOR_CLOSING: Serial.println("DOOR_CLOSING"); break;
      case DOOR_OPEN: Serial.println("DOOR_OPEN"); break;
      case CALIBRATING: Serial.println("CALIBRATING"); break;
      case CALIBRATION_FAILED: Serial.println("CALIBRATION_FAILED"); break;
      case EMERGENCY_STOP: Serial.println("EMERGENCY_STOP"); break;
      case EMERGENCY_RESETTING: Serial.println("EMERGENCY_RESETTING"); break;
    }
    lastState = currentState;
  }

  // Door control logic
  if (currentState >= DOOR_OPENING && currentState <= DOOR_OPEN) {
    handleDoorControl();
  } else {
    // Movement logic
    switch (currentState) {
      case IDLE: {
        int nextFloor = findNextFloor();
        if (nextFloor != -1) {
          if (nextFloor > currentFloor) {
            currentState = MOVING_UP;
            lastDirection = MOVING_UP;
          } else {
            currentState = MOVING_DOWN;
            lastDirection = MOVING_DOWN;
          }
          motorStartTime = millis();
        }
        break;
      }
      case MOVING_UP:
      case MOVING_DOWN:
        controlMotor(currentState);
        break;
      case EMERGENCY_RESETTING:
        emergencyReset();
        break;
      default:
        break;
    }
  }
}

void handleTestMode() {
  processCallButtons();
  processDoorAndLightButtons();
  handleLimitSwitches();
  handleMotorTimeout();

  static ElevatorState lastState = currentState;
  if (lastState != currentState) {
    Serial.print("State berubah menjadi: ");
    switch (currentState) {
      case IDLE: Serial.println("IDLE"); break;
      case MOVING_UP: Serial.println("MOVING_UP"); break;
      case MOVING_DOWN: Serial.println("MOVING_DOWN"); break;
      case DOOR_OPENING: Serial.println("DOOR_OPENING"); break;
      case DOOR_CLOSING: Serial.println("DOOR_CLOSING"); break;
      case DOOR_OPEN: Serial.println("DOOR_OPEN"); break;
      case CALIBRATING: Serial.println("CALIBRATING"); break;
      case CALIBRATION_FAILED: Serial.println("CALIBRATION_FAILED"); break;
      case EMERGENCY_STOP: Serial.println("EMERGENCY_STOP"); break;
      case EMERGENCY_RESETTING: Serial.println("EMERGENCY_RESETTING"); break;
    }
    lastState = currentState;
  }

  if (currentState >= DOOR_OPENING && currentState <= DOOR_OPEN) {
    handleDoorControl();
  } else {
    switch (currentState) {
      case IDLE: {
        int nextFloor = findNextFloor();
        if (nextFloor != -1) {
          if (nextFloor > currentFloor) {
            currentState = MOVING_UP;
            lastDirection = MOVING_UP;
          } else {
            currentState = MOVING_DOWN;
            lastDirection = MOVING_DOWN;
          }
          motorStartTime = millis();
        }
        break;
      }
      case MOVING_UP:
      case MOVING_DOWN:
        controlMotor(currentState);
        break;
      case EMERGENCY_RESETTING:
        emergencyReset();
        break;
      default:
        break;
    }
  }
}

void handleInspectionMode() {
  unsigned long now = millis();
  bool inspUpButtonState = (bitRead(inputData[INSP_UP_BUTTON.srIndex], INSP_UP_BUTTON.bitPosition) == 0);
  bool inspDownButtonState = (bitRead(inputData[INSP_DOWN_BUTTON.srIndex], INSP_DOWN_BUTTON.bitPosition) == 0);

  if (inspUpButtonState && (now - lastInspUpTime) > DEBOUNCE_DELAY_MS) {
    if (currentState == IDLE) {
      currentState = MOVING_UP;
      lastDirection = MOVING_UP;
      motorStartTime = now;
      Serial.println("Tombol INSPEKSI NAIK ditekan.");
    }
    lastInspUpTime = now;
  }
  if (inspDownButtonState && (now - lastInspDownTime) > DEBOUNCE_DELAY_MS) {
    if (currentState == IDLE) {
      currentState = MOVING_DOWN;
      lastDirection = MOVING_DOWN;
      motorStartTime = now;
      Serial.println("Tombol INSPEKSI TURUN ditekan.");
    }
    lastInspDownTime = now;
  }
  if (!inspUpButtonState && !inspDownButtonState) {
    if (currentState == MOVING_UP || currentState == MOVING_DOWN) {
      stopMotor();
      currentState = IDLE;
      Serial.println("Kedua tombol INSPEKSI dilepas. Motor dihentikan.");
    }
  }

  if (Serial2.available() > 0) {
    String command = Serial2.readStringUntil('\n');
    command.trim();
    lastSerialDataTime = millis();
    if (command.equals("UP")) {
      if (currentState == IDLE) {
        currentState = MOVING_UP;
        lastDirection = MOVING_UP;
        motorStartTime = now;
        Serial.println("Perintah INSPEKSI: NAIK");
      }
    } else if (command.equals("DOWN")) {
      if (currentState == IDLE) {
        currentState = MOVING_DOWN;
        lastDirection = MOVING_DOWN;
        motorStartTime = now;
      Serial.println("Perintah INSPEKSI: TURUN");
      }
    } else if (command.equals("STOP")) {
      stopMotor();
      currentState = IDLE;
      Serial.println("Perintah INSPEKSI: STOP");
    }
  }

  switch (currentState) {
    case MOVING_UP:
    case MOVING_DOWN:
      controlMotor(currentState);
      break;
    case IDLE:
      // Don't call stopMotor() here as it might be called repeatedly
      break;
    default:
      break;
  }

  handleLimitSwitches();
  handleMotorTimeout();
}

// =================================== CORE LOGIC FUNCTIONS ===================================
void calibrateLift() {
  Serial.println("Memulai kalibrasi/homing...");
  unsigned long startTime = millis();
  currentState = CALIBRATING;
  stopMotor();

  readAllSRInputs();
  if (getLimitSwitchStatus(1)) {
    currentFloor = 1;
    Serial.println("Kalibrasi OK. Lift sudah berada di lantai 1.");
    currentState = IDLE;
    return;
  }

  Serial.println("Homing: Bergerak turun ke limit switch Lantai 1...");
  digitalWrite(RELAY_MOTOR_DOWN, HIGH);
  digitalWrite(RELAY_MOTOR_BRAKE, LOW);

  unsigned long lastProgressUpdate = 0;
  int dotCount = 0;
  const unsigned long PROGRESS_UPDATE_INTERVAL = 250;

  while (!getLimitSwitchStatus(1) && (millis() - startTime) < MOTOR_TIMEOUT_MS) {
    readAllSRInputs();
    unsigned long now = millis();
    if (now - lastProgressUpdate > PROGRESS_UPDATE_INTERVAL) {
      lastProgressUpdate = now;
      dotCount = (dotCount + 1) % 4;
      display.clearDisplay();
      display.setTextSize(1);
      display.setCursor(0, 29);
      display.println("Homing to Reference");
      display.setCursor(0, 39);
      display.print("Mohon tunggu");
      for (int i = 0; i < dotCount; i++) display.print(".");
      display.display();
      
      unsigned long elapsedTime = millis() - animationStartTime;
    int barWidth = map(elapsedTime, 0, 10000, 0, SCREEN_WIDTH);
    if (barWidth > SCREEN_WIDTH) barWidth = SCREEN_WIDTH;

    display.drawRect(0, 50, SCREEN_WIDTH, 5, WHITE);
    display.fillRect(0, 51, barWidth, 3, WHITE);
    display.display();
    delay(10);
  }

  stopMotor();
  readAllSRInputs();
  if (getLimitSwitchStatus(1)) {
    currentFloor = 1;
    Serial.println("Kalibrasi Berhasil. Posisi: Lantai 1");
    currentState = IDLE;
  } else {
    currentState = CALIBRATION_FAILED;
    Serial.println("Kalibrasi GAGAL! Timeout terlampaui. Periksa kabel atau limit switch.");
  }
}

void emergencyReset() {
  Serial.println("Lift mengalami keadaan darurat.");
  Serial.println("Memulai kalibrasi/homing otomatis ke Lantai 1...");
  isEmergency = false;
  calibrateLift();
  if (currentState != CALIBRATION_FAILED) {
    currentState = IDLE;
    Serial.println("Reset darurat selesai. Sistem kembali IDLE.");
  }
}

void readDipSwitch() {
  int dipValue = 0;
  dipValue |= (digitalRead(DIP_PIN_0) == LOW) ? 1 : 0;
  dipValue |= (digitalRead(DIP_PIN_1) == LOW) ? 2 : 0;
  dipValue |= (digitalRead(DIP_PIN_2) == LOW) ? 4 : 0;
  dipValue |= (digitalRead(DIP_PIN_3) == LOW) ? 8 : 0;
  totalFloors = dipValue;
  if (totalFloors < 2 || totalFloors > TOTAL_MAX_FLOORS) totalFloors = 3;
  Serial.print("Total Floors: ");
  Serial.println(totalFloors);
}

// =================================== MOTOR CONTROL (NON-BLOCKING) ===================================
void stopMotor() {
  digitalWrite(RELAY_MOTOR_UP, LOW);
  digitalWrite(RELAY_MOTOR_DOWN, LOW);
  motorStopping = true;
  stopStartTime = millis();
  Serial.println("Motor stopping initiated (non-blocking).");
}

void controlMotor(ElevatorState state) {
  if (isEmergency || finalLimitInterrupted || currentState == EMERGENCY_STOP) {
    Serial.println("Motor command blocked (emergency / final limit).");
    stopMotor();
    return;
  }
  
  if (state == MOVING_UP && currentFloor >= totalFloors) {
    Serial.println("DI ATAS BATAS! Stop.");
    stopMotor();
    currentState = IDLE;
    return;
  }
  if (state == MOVING_DOWN && currentFloor <= 1) {
    Serial.println("DI BAWAH BATAS! Stop.");
    stopMotor();
    currentState = IDLE;
    return;
  }

  if ((state == MOVING_UP && lastDirection == MOVING_DOWN) || (state == MOVING_DOWN && lastDirection == MOVING_UP)) {
    stopMotor();
    waitingSwitch = true;
    switchStartTime = millis();
    nextDirection = state;
    Serial.println("Switching arah motor...");
    return;
  }

  if (state == MOVING_UP) {
    digitalWrite(RELAY_MOTOR_DOWN, LOW);
    digitalWrite(RELAY_MOTOR_UP, HIGH);
    digitalWrite(RELAY_MOTOR_BRAKE, LOW);
    motorStartTime = millis();
    lastDirection = MOVING_UP;
    Serial.println("Motor UP aktif.");
  } else if (state == MOVING_DOWN) {
    digitalWrite(RELAY_MOTOR_UP, LOW);
    digitalWrite(RELAY_MOTOR_DOWN, HIGH);
    digitalWrite(RELAY_MOTOR_BRAKE, LOW);
    motorStartTime = millis();
    lastDirection = MOVING_DOWN;
    Serial.println("Motor DOWN aktif.");
  }
}

void updateMotorControl() {
  unsigned long now = millis();

  if (motorStopping && (now - stopStartTime >= STOP_DELAY_MS)) {
    digitalWrite(RELAY_MOTOR_BRAKE, HIGH);
    motorStopping = false;
    Serial.println("Motor brake engaged.");
  }

  if (waitingSwitch && (now - switchStartTime >= MOTOR_DIRECTION_SWITCH_DELAY_MS)) {
    waitingSwitch = false;
    controlMotor(nextDirection);
  }
}


// =================================== AUXILIARY FUNCTIONS ===================================
void displayStatus() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  if (currentState == CALIBRATION_FAILED) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(5, 0);
    display.println("--- HOMING ERROR ---");
    display.setTextSize(1);
    display.setCursor(0, 40);
    display.print("Segera periksa kabel atau limit switch");
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= 500) {
      previousMillis = currentMillis;
      dotCount = (dotCount + 1) % 4;
    }
    int dotX = display.getCursorX();
    int dotY = display.getCursorY();
    for (int i = 0; i < dotCount; i++) {
      display.setCursor(dotX + (i * 6), dotY);
      display.print(".");
    }
    display.display();
    return;
  }
  if (currentState == CALIBRATING) return;
  if (currentState == EMERGENCY_STOP) {
    if (millis() - lastBlink > 250) {
      blinkState = !blinkState;
      lastBlink = millis();
    }
    if (blinkState) {
      display.clearDisplay();
      display.fillRect(0, 0, SCREEN_WIDTH, 40, WHITE);
      display.setTextColor(BLACK);
      display.setCursor(17, 4);
      display.setTextSize(2);
      display.print("LANTAI ");
      display.println(currentFloor);
      display.setCursor(17, 21);
      display.setTextSize(2);
      display.println("DARURAT!");
    } else {
      display.clearDisplay();
    }
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 47);
    display.println("DO NOT MOVE!");
    display.setCursor(0, 57);
    display.println("CALLING MAINTENANCE.");
    display.display();
    return;
  }

  display.setCursor(0, 0);
  display.println("Orch_ Elevator");
  display.drawLine(0, 9, SCREEN_WIDTH - 1, 9, WHITE);
  display.setCursor(0, 47);
  display.print("MODE: ");
  switch (currentMode) {
    case MODE_NORMAL: display.println("NORMAL/AUTO"); break;
    case MODE_TEST: display.println("TEST"); break;
    case MODE_INSPECTION: display.println("INSPEKSI"); break;
  }
  display.setTextSize(3);
  display.setCursor(0, 18);
  display.print("F:");
  display.println(currentFloor);
  display.setCursor(0, 57);
  display.setTextSize(1);

  if (currentState == DOOR_OPENING || currentState == DOOR_CLOSING) {
    display.print("DOOR: ");
    if (currentState == DOOR_OPENING) display.print("OPENING");
    else display.print("CLOSING");
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= 500) {
      previousMillis = currentMillis;
      dotCount = (dotCount + 1) % 4;
    }
    for (int i = 0; i < dotCount; i++) display.print(".");
  } else if (currentState == DOOR_OPEN) {
    display.print("DOOR: OPEN, Rem:(");
    unsigned long elapsed = millis() - stateTimer;
    int remaining = (DOOR_OPEN_HOLD_TIME_MS - elapsed) / 1000;
    if (remaining < 0) remaining = 0;
    display.print(remaining);
    display.print("s)");
  } else {
    display.print("CALLS:");
    displayCallQueue();
  }

  if (currentState == MOVING_UP || currentState == MOVING_DOWN) {
    if (millis() - lastBlink > 250) {
      blinkState = !blinkState;
      lastBlink = millis();
    }
    if (blinkState) {
      if (currentState == MOVING_UP) drawArrowUp(100, 20, 20);
      else drawArrowDown(100, 20, 20);
    }
  }
  display.display();
}

void displayCallQueue() {
  int y = 57, x = 47, colW = 25, rowH = 10;
  int callCount = 0;
  bool anyCall = false;
  for (int i = 0; i < totalFloors; i++) {
    if (callUp[i] || callDown[i] || copCallActive[i]) {
      display.setCursor(x + (callCount % 3) * colW, y + (callCount / 3) * rowH);
      display.print("F");
      display.print(i + 1);
      callCount++;
      anyCall = true;
    }
  }
  if (!anyCall) {
    display.setCursor(x, y);
    display.print("NONE");
  }
}

void processDoorAndLightButtons() {
  unsigned long now = millis();
  bool openButton = (bitRead(inputData[DOOR_OPEN_BUTTON.srIndex], DOOR_OPEN_BUTTON.bitPosition) == 0);
  bool closeButton = (bitRead(inputData[DOOR_CLOSE_BUTTON.srIndex], DOOR_CLOSE_BUTTON.bitPosition) == 0);

  if (openButton && now - lastOpenButtonTime > DEBOUNCE_DELAY_MS) {
    if (currentState == DOOR_CLOSING || currentState == IDLE || currentState == DOOR_OPEN) {
      currentState = DOOR_OPENING;
      doorTimer = now;
      lastActivityTime = now;
      Serial.println("Tombol Buka Pintu ditekan.");
    }
    lastOpenButtonTime = now;
  }
  if (closeButton && now - lastCloseButtonTime > DEBOUNCE_DELAY_MS) {
    if (currentState == DOOR_OPEN || currentState == DOOR_OPENING) {
      currentState = DOOR_CLOSING;
      doorTimer = now;
      lastActivityTime = now;
      Serial.println("Tombol Tutup Pintu ditekan.");
    }
    lastCloseButtonTime = now;
  }
  bool currentCabinLightButtonState = (bitRead(inputData[CABIN_LIGHT_BUTTON.srIndex], CABIN_LIGHT_BUTTON.bitPosition) == 0);
  if (lastCabinLightButtonState == HIGH && currentCabinLightButtonState == LOW && (now - lastCabinLightTime) > DEBOUNCE_DELAY_MS) {
    cabinLightOn = !cabinLightOn;
    lastCabinLightTime = now;
    lastActivityTime = now;
    Serial.print("Lampu Kabin: ");
    Serial.println(cabinLightOn ? "ON" : "OFF");
  }
  lastCabinLightButtonState = currentCabinLightButtonState;
}

void handleDoorControl() {
  switch (currentState) {
    case DOOR_OPENING:
      if (!doorCommandSent) {
        Serial2.print("OPEN\n");
        doorCommandSent = true;
        Serial.println("Mengirim perintah buka pintu...");
      }
      if (millis() - doorTimer > DOOR_OPEN_DURATION_MS) {
        currentState = DOOR_OPEN;
        doorTimer = millis();
        doorCommandSent = false;
        Serial.println("Pintu terbuka sepenuhnya.");
      }
      break;
    case DOOR_OPEN:
      if ((millis() - doorTimer > DOOR_OPEN_HOLD_TIME_MS) || (bitRead(inputData[DOOR_CLOSE_BUTTON.srIndex], DOOR_CLOSE_BUTTON.bitPosition) == 0 && (millis() - lastCloseButtonTime) > DEBOUNCE_DELAY_MS)) {
        currentState = DOOR_CLOSING;
        doorTimer = millis();
        Serial.println("Waktu tunggu pintu habis atau tombol tutup ditekan, memulai penutupan.");
      }
      break;
    case DOOR_CLOSING:
      if (!doorCommandSent) {
        Serial2.print("CLOSE\n");
        doorCommandSent = true;
        Serial.println("Mengirim perintah tutup pintu...");
      }
      if (millis() - doorTimer > DOOR_CLOSE_DURATION_MS) {
        currentState = IDLE;
        doorCommandSent = false;
        Serial.println("Pintu tertutup. Lift kembali ke status IDLE.");
      }
      break;
  }
}

void processCallButtons() {
  unsigned long now = millis();
  for (int i = 0; i < NUM_COP_BUTTONS; i++) {
    bool reading = (bitRead(inputData[COP_BUTTONS[i].srIndex], COP_BUTTONS[i].bitPosition) == 0);
    if (reading && (now - lastButtonPressTime[i]) > DEBOUNCE_DELAY_MS) {
      int floorIndex = i;
      if (floorIndex + 1 == currentFloor) {
        if (currentState == IDLE || currentState == DOOR_OPEN) {
          Serial.println("Tombol lantai saat ini ditekan. Memicu pembukaan pintu.");
          currentState = DOOR_OPENING;
          doorTimer = now;
        }
      } else {
        copCallActive[floorIndex] = true;
        Serial.print("Panggilan COP ke Lantai ");
        Serial.print(floorIndex + 1);
        Serial.println(" ditambahkan ke antrean.");
      }
      lastButtonPressTime[i] = now;
      lastActivityTime = now;
    }
  }

  for (int i = 0; i < NUM_LOP_BUTTONS; i++) {
    bool callUpButtonState = (bitRead(inputData[LOP_UP_BUTTONS[i].srIndex], LOP_UP_BUTTONS[i].bitPosition) == 0);
    if (callUpButtonState && (now - lastCallUpTime) > DEBOUNCE_DELAY_MS) {
      if ((i + 1) < totalFloors) {
        callUp[i] = true;
        lopCallActive[i] = true;
        Serial.print("Panggilan LOP Naik dari Lantai ");
        Serial.print(i + 1);
        Serial.println(" ditambahkan ke antrean.");
      }
      lastCallUpTime = now;
      lastActivityTime = now;
    }

    bool callDownButtonState = (bitRead(inputData[LOP_DOWN_BUTTONS[i].srIndex], LOP_DOWN_BUTTONS[i].bitPosition) == 0);
    if (callDownButtonState && (now - lastCallDownTime) > DEBOUNCE_DELAY_MS) {
      if ((i + 1) > 1) {
        callDown[i] = true;
        lopCallActive[i] = true;
        Serial.print("Panggilan LOP Turun dari Lantai ");
        Serial.print(i + 1);
        Serial.println(" ditambahkan ke antrean.");
      }
      lastCallDownTime = now;
      lastActivityTime = now;
    }
  }
}

int findNextFloor() {
  if (lastDirection == MOVING_UP) {
    for (int i = currentFloor; i < totalFloors; i++) {
      if (copCallActive[i] || callUp[i] || callDown[i]) return i + 1;
    }
    lastDirection = MOVING_DOWN;
    for (int i = currentFloor - 2; i >= 0; i--) {
      if (copCallActive[i] || callUp[i] || callDown[i]) return i + 1;
    }
  } else {
    for (int i = currentFloor - 2; i >= 0; i--) {
      if (copCallActive[i] || callUp[i] || callDown[i]) return i + 1;
    }
    lastDirection = MOVING_UP;
    for (int i = currentFloor; i < totalFloors; i++) {
      if (copCallActive[i] || callUp[i] || callDown[i]) return i + 1;
    }
  }
  return -1;
}

void handleLimitSwitches() {
  unsigned long now = millis();
  for (int i = 0; i < totalFloors; i++) {
    bool currentLimitStatus = getLimitSwitchStatus(i + 1);
    if (currentLimitStatus != lastLimitStatus[i] && (now - lastLimitDebounceTime[i]) > LIMIT_DEBOUNCE_DELAY_MS) {
      if (currentLimitStatus) {
        currentFloor = i + 1;
        bool isDestination = copCallActive[i] || (lastDirection == MOVING_UP && callUp[i]) || (lastDirection == MOVING_DOWN && callDown[i]);
        if (isDestination || (currentFloor == totalFloors && lastDirection == MOVING_UP) || (currentFloor == 1 && lastDirection == MOVING_DOWN)) {
          stopMotor();
          currentState = DOOR_OPENING;
          doorTimer = now;
          lastActivityTime = now;
          if (isDestination) {
            copCallActive[i] = callUp[i] = callDown[i] = false;
          }
          Serial.print("Mencapai lantai tujuan ");
          Serial.print(currentFloor);
          Serial.println(". Pintu akan terbuka.");
          Serial1.print(currentFloor);
          Serial1.print("\n");
        } else {
          Serial.print("Lift melewati lantai ");
          Serial.print(currentFloor);
          Serial.println(" tanpa berhenti.");
        }
        motorStartTime = now;
      }
      lastLimitStatus[i] = currentLimitStatus;
      lastLimitDebounceTime[i] = now;
    }
  }
}

void handleMotorTimeout() {
  if ((currentState == MOVING_UP || currentState == MOVING_DOWN) && (millis() - motorStartTime > MOTOR_TIMEOUT_MS)) {
    Serial.println("Motor timeout! Lift tidak mencapai lantai dalam waktu yang ditentukan.");
    isEmergency = true;
    currentState = EMERGENCY_STOP;
    stopMotor();
  }
}

void handleEmergency() {
  bool emergencyPinState = digitalRead(EMERGENCY_PIN) == LOW;
  if (emergencyPinState && !isEmergency) {
    isEmergency = true;
    currentState = EMERGENCY_STOP;
    stopMotor();
    Serial.println("Tombol darurat ditekan!");
  } else if (!emergencyPinState && isEmergency) {
    isEmergency = false;
    currentState = EMERGENCY_RESETTING;
    Serial.println("Reset darurat.");
  }
}

void drawArrowUp(int x, int y, int size) {
  display.fillTriangle(x, y + size, x + size, y + size, x + size / 2, y, WHITE);
}

void drawArrowDown(int x, int y, int size) {
  display.fillTriangle(x, y, x + size, y, x + size / 2, y + size, WHITE);
}

void showStartup() {
  display.clearDisplay();
  display.setTextSize(3);
  display.setCursor(120, 0);
  display.setTextColor(SSD1306_WHITE);
  display.print("Orch.Gen");
  display.display();
  for (int x = 120; x > 33; x -= 2) {
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(x, 0);
    display.print("Orch_");
    display.println("");
    display.print(" ELEVATOR");
    display.display();
    delay(10);
  }
  display.setTextSize(1);
  display.setCursor(31, 50);
  display.print("By Elzabid");
  display.display();
  delay(150);
}

void readAllSRInputs() {
  digitalWrite(SR_IN_LATCH, LOW);
  delayMicroseconds(5);
  digitalWrite(SR_IN_LATCH, HIGH);
  inputData[3] = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
  inputData[2] = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
  inputData[1] = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
  inputData[0] = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
}

bool getLimitSwitchStatus(int floor) {
  if (floor < 1 || floor > TOTAL_MAX_FLOORS) return false;
  ShiftRegisterPin limitPin = FLOOR_LIMIT_SWITCHES[floor - 1];
  return bitRead(inputData[limitPin.srIndex], limitPin.bitPosition) == 0;
}

void writeAllSROutputs() {
  digitalWrite(SR_OUT_LATCH, LOW);
  shiftOut(SR_OUT_DATA, SR_OUT_CLOCK, MSBFIRST, outputData[1]);
  shiftOut(SR_OUT_DATA, SR_OUT_CLOCK, MSBFIRST, outputData[0]);
  digitalWrite(SR_OUT_LATCH, HIGH);
}

void updateFloorLeds() {
  outputData[0] = 0;
  outputData[1] = 0;

  if (isEmergency || currentState == CALIBRATION_FAILED || finalLimitInterrupted) {
    bitSet(outputData[ERROR_LED.srIndex], ERROR_LED.bitPosition);
    bitClear(outputData[SYSTEM_OK_LED.srIndex], SYSTEM_OK_LED.bitPosition);
  } else {
    bitSet(outputData[SYSTEM_OK_LED.srIndex], SYSTEM_OK_LED.bitPosition);
    bitClear(outputData[ERROR_LED.srIndex], ERROR_LED.bitPosition);
  }

  bool anyCopCall = false;
  bool anyLopCall = false;
  for (int i = 0; i < totalFloors; i++) {
    if (copCallActive[i]) {
      anyCopCall = true;
      bitSet(outputData[CALL_LEDS[i].srIndex], CALL_LEDS[i].bitPosition);
    }
    if (lopCallActive[i]) {
      anyLopCall = true;
    }
  }

  if (anyCopCall) bitSet(outputData[COP_CALL_LED.srIndex], COP_CALL_LED.bitPosition);
  else bitClear(outputData[COP_CALL_LED.srIndex], COP_CALL_LED.bitPosition);

  if (anyLopCall) bitSet(outputData[LOP_CALL_LED.srIndex], LOP_CALL_LED.bitPosition);
  else bitClear(outputData[LOP_CALL_LED.srIndex], LOP_CALL_LED.bitPosition);

  if (millis() - lastSerialDataTime < SERIAL_TIMEOUT_MS) bitSet(outputData[SER_COM_OK_LED.srIndex], SER_COM_OK_LED.bitPosition);
  else bitClear(outputData[SER_COM_OK_LED.srIndex], SER_COM_OK_LED.bitPosition);

  if (currentState == DOOR_OPENING || currentState == DOOR_CLOSING) {
    if (millis() % 500 < 250) bitSet(outputData[DOOR_LED.srIndex], DOOR_LED.bitPosition);
    else bitClear(outputData[DOOR_LED.srIndex], DOOR_LED.bitPosition);
  } else if (currentState == DOOR_OPEN) {
    bitSet(outputData[DOOR_LED.srIndex], DOOR_LED.bitPosition);
  } else {
    bitClear(outputData[DOOR_LED.srIndex], DOOR_LED.bitPosition);
  }

  if (currentMode == MODE_NORMAL) {
    if (anyCopCall || anyLopCall || currentState == MOVING_UP || currentState == MOVING_DOWN || (millis() - lastActivityTime) < CABIN_LIGHT_TIMEOUT_MS) {
      cabinLightOn = true;
    } else {
      cabinLightOn = false;
    }
  } else {
    if ((millis() - lastActivityTime) < CABIN_LIGHT_TIMEOUT_MS) cabinLightOn = true;
  }

  if (cabinLightOn) bitSet(outputData[CABIN_LIGHT_LED.srIndex], CABIN_LIGHT_LED.bitPosition);
  else bitClear(outputData[CABIN_LIGHT_LED.srIndex], CABIN_LIGHT_LED.bitPosition);

  if (isEmergency || currentState == CALIBRATION_FAILED) bitSet(outputData[BUZZER_PIN.srIndex], BUZZER_PIN.bitPosition);
  else bitClear(outputData[BUZZER_PIN.srIndex], BUZZER_PIN.bitPosition);

  writeAllSROutputs();
}