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

// --- Definisi Pin ESP32 ---
#define DIP_PIN_0 13
#define DIP_PIN_1 14
#define DIP_PIN_2 15
#define DIP_PIN_3 27

#define TEST_MODE_PIN 35
#define EMERGENCY_PIN 23

// UART pintu
#define SERIAL_DOOR_RX 16
#define SERIAL_DOOR_TX 17

// Relay motor
#define RELAY_MOTOR_UP 19
#define RELAY_MOTOR_DOWN 18
#define RELAY_MOTOR_BRAKE 4

// OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- Variabel Global ---
int totalFloors = 3;
int currentFloor = 0;

// Mapping tombol & limit switch (max 16 lantai)
const int buttonPins[16] = {34, 33, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
const int limitPins[16] = {25, 26, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

// Logika antrean ganda untuk permintaan naik dan turun
bool callUp[16] = {false};
bool callDown[16] = {false};

// State machine
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;

// Mode operasi
enum OperationMode {
  MODE_NORMAL,
  MODE_TEST,
  MODE_INSPECTION
};
OperationMode currentMode = MODE_NORMAL;

bool isEmergency = false;
unsigned long doorTimer = 0;
const unsigned long doorOpenDuration = 3000;
const unsigned long doorCloseDuration = 2000;
const unsigned long DOOR_OPEN_HOLD_TIME = 5000; // Waktu pintu terbuka penuh (tahan)
const unsigned long MOTOR_DIRECTION_SWITCH_DELAY_MS = 200;

// Variabel untuk memastikan log hanya tampil sekali
bool logDoorOpening = false;
bool logDoorClosing = false;
bool logEmergency = false;
bool logDoorOpen = false;

// --- Debounce tombol yang lebih baik ---
const unsigned long debounceDelay = 50;
unsigned long lastButtonPressTime[16] = {0};
bool buttonState[16];

// --- Edge detection limit switch ---
bool lastLimitState[16];

// --- Timing relay ---
const unsigned long RELAY_SETTLE_MS = 30;

// --- Variabel debounce tombol mode ---
bool lastTestModePinState = HIGH;
unsigned long lastModeChangeTime = 0;
const unsigned long modeDebounceDelay = 200;

// --- Variabel untuk tampilan dinamis (revisi) ---
unsigned long lastBlink = 0;
bool blinkState = true;

// Variabel untuk animasi titik di Homing Gagal
unsigned long previousMillis = 0;
int dotCount = 0;

// --- Prototipe ---
void readDipSwitch();
void controlMotor(ElevatorState state);
void stopMotor();
void displayStatus();
void displayCallQueue();
void processCallButtons();
void handleLimitSwitches();
void handleEmergency();
void calibrateLift();
void emergencyReset();
bool isAnyCall();
int findNextFloor();
void showStartup();
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 setup() {
  Serial.begin(115200);
  Serial.println("Elevator Controller Starting...");

  pinMode(DIP_PIN_0, INPUT_PULLUP);
  pinMode(DIP_PIN_1, INPUT_PULLUP);
  pinMode(DIP_PIN_2, INPUT_PULLUP);
  pinMode(DIP_PIN_3, INPUT_PULLUP);
  pinMode(TEST_MODE_PIN, INPUT_PULLUP);
  pinMode(EMERGENCY_PIN, INPUT_PULLUP);

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

  for (int i = 0; i < 16; i++) {
    if (buttonPins[i] != 0) {
      pinMode(buttonPins[i], INPUT_PULLUP);
      buttonState[i] = digitalRead(buttonPins[i]);
    }
    if (limitPins[i] != 0) {
      pinMode(limitPins[i], INPUT_PULLUP);
      lastLimitState[i] = digitalRead(limitPins[i]);
    }
  }

  Serial2.begin(9600, SERIAL_8N1, SERIAL_DOOR_RX, SERIAL_DOOR_TX);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  showStartup();
  readDipSwitch();
  calibrateLift();
  if (currentState != CALIBRATION_FAILED) {
    currentState = IDLE;
    Serial.println("System is ready. State: IDLE");
  }
}

void loop() {
  handleEmergency();

  if (isEmergency) {
    stopMotor();
    displayStatus();
    return;
  }

  if (currentState == CALIBRATION_FAILED) {
    displayStatus();
    return;
  }

  handleModeChange();

  switch (currentMode) {
    case MODE_NORMAL:
      handleNormalMode();
      break;
    case MODE_TEST:
      handleTestMode();
      break;
    case MODE_INSPECTION:
      handleInspectionMode();
      break;
    default:
      currentMode = MODE_NORMAL;
      break;
  }
  displayStatus();
  delay(20);
}

// ================== Fungsi Mode Operasi ==================
void handleModeChange() {
  bool currentTestModePinState = digitalRead(TEST_MODE_PIN);
  unsigned long now = millis();

  if (lastTestModePinState == HIGH && currentTestModePinState == LOW && (now - lastModeChangeTime) > modeDebounceDelay) {
    switch (currentMode) {
      case MODE_NORMAL:
        currentMode = MODE_TEST;
        Serial.println("Mode changed to: TEST");
        break;
      case MODE_TEST:
        currentMode = MODE_INSPECTION;
        Serial.println("Mode changed to: INSPECTION");
        break;
      case MODE_INSPECTION:
        currentMode = MODE_NORMAL;
        Serial.println("Mode changed to: NORMAL");
        break;
    }
    lastModeChangeTime = now;
    stopMotor();
    currentState = IDLE;
    for (int i = 0; i < totalFloors; i++) {
      callUp[i] = false;
      callDown[i] = false;
    }
  }
  lastTestModePinState = currentTestModePinState;
}

void handleNormalMode() {
  if (Serial2.available() > 0) {
    String command = Serial2.readStringUntil('\n');
    command.trim();
    if (command.startsWith("CALL")) {
      int floor = command.substring(4).toInt();
      if (floor >= 1 && floor <= totalFloors) {
        if (floor > currentFloor) {
          callUp[floor - 1] = true;
          Serial.print("Panggilan UART ke Lantai "); Serial.print(floor); Serial.println(" (NAIK)");
        } else if (floor < currentFloor) {
          callDown[floor - 1] = true;
          Serial.print("Panggilan UART ke Lantai "); Serial.print(floor); Serial.println(" (TURUN)");
        } else {
          if (currentState == IDLE || currentState == DOOR_OPEN || currentState == DOOR_OPENING) {
            currentState = DOOR_OPENING;
            doorTimer = millis();
            Serial.println("Panggilan ke lantai saat ini. Memicu pembukaan pintu.");
          }
        }
      }
    }
  }

  handleLimitSwitches();

  static ElevatorState lastState = currentState;
  if (lastState != currentState) {
    logDoorOpening = false;
    logDoorClosing = false;
    logDoorOpen = false;
    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;
    }
  }

  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;
        }
      }
      break;
    }
    case MOVING_UP:
    case MOVING_DOWN:
      controlMotor(currentState);
      break;
    case DOOR_OPENING:
      if (!logDoorOpening) {
        Serial.println("PINTU SEDANG TERBUKA");
        Serial2.print("OPEN\n");
        doorTimer = millis();
        logDoorOpening = true;
      }
      stopMotor();
      if (millis() - doorTimer > doorOpenDuration) {
        currentState = DOOR_OPEN;
        doorTimer = millis(); // Mencatat waktu mulai pintu terbuka penuh
      }
      break;
    case DOOR_OPEN:
      if (!logDoorOpen) {
        Serial.println("PINTU SUDAH TERBUKA PENUH");
        logDoorOpen = true;
      }
      if (millis() - doorTimer > DOOR_OPEN_HOLD_TIME) {
        currentState = DOOR_CLOSING;
        logDoorClosing = false; // Reset log untuk memulai proses penutupan
      }
      break;
    case DOOR_CLOSING: {
      if (!logDoorClosing) {
        Serial.println("PINTU SEDANG TERTUTUP");
        Serial2.print("CLOSE\n");
        doorTimer = millis(); // Menggunakan kembali doorTimer untuk penutupan
        logDoorClosing = true;
      }
      if (millis() - doorTimer > doorCloseDuration) { // Menggunakan durasi yang sama untuk demo
        int nextFloor = findNextFloor();
        if (nextFloor != -1) {
          if (nextFloor > currentFloor) {
            currentState = MOVING_UP;
            lastDirection = MOVING_UP;
          } else {
            currentState = MOVING_DOWN;
            lastDirection = MOVING_DOWN;
          }
        } else {
          currentState = IDLE;
        }
      }
      break;
    }
    case EMERGENCY_RESETTING:
      emergencyReset();
      break;
    default:
      break;
  }
}

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

  static ElevatorState lastState = currentState;
  if (lastState != currentState) {
    logDoorOpening = false;
    logDoorClosing = false;
    logDoorOpen = false;
    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;
    }
  }

  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;
        }
      }
      break;
    }
    case MOVING_UP:
    case MOVING_DOWN:
      controlMotor(currentState);
      break;
    case DOOR_OPENING:
      if (!logDoorOpening) {
        Serial.println("PINTU SEDANG TERBUKA");
        Serial2.print("OPEN\n");
        doorTimer = millis();
        logDoorOpening = true;
      }
      stopMotor();
      if (millis() - doorTimer > doorOpenDuration) {
        currentState = DOOR_OPEN;
        doorTimer = millis();
      }
      break;
    case DOOR_OPEN:
      if (!logDoorOpen) {
        Serial.println("PINTU SUDAH TERBUKA PENUH");
        logDoorOpen = true;
      }
      if (millis() - doorTimer > DOOR_OPEN_HOLD_TIME) {
        currentState = DOOR_CLOSING;
        logDoorClosing = false;
      }
      break;
    case DOOR_CLOSING: {
      if (!logDoorClosing) {
        Serial.println("PINTU SEDANG TERTUTUP");
        Serial2.print("CLOSE\n");
        doorTimer = millis();
        logDoorClosing = true;
      }
      if (millis() - doorTimer > doorCloseDuration) {
        int nextFloor = findNextFloor();
        if (nextFloor != -1) {
          if (nextFloor > currentFloor) {
            currentState = MOVING_UP;
            lastDirection = MOVING_UP;
          } else {
            currentState = MOVING_DOWN;
            lastDirection = MOVING_DOWN;
          }
        } else {
          currentState = IDLE;
        }
      }
      break;
    }
    case EMERGENCY_RESETTING:
      emergencyReset();
      break;
    default:
      break;
  }
}

void handleInspectionMode() {
  if (Serial2.available() > 0) {
    String command = Serial2.readStringUntil('\n');
    command.trim();
    if (command.equals("UP")) {
      if (currentState == IDLE) {
        currentState = MOVING_UP;
        lastDirection = MOVING_UP;
        Serial.println("Perintah INSPEKSI: NAIK");
      }
    } else if (command.equals("DOWN")) {
      if (currentState == IDLE) {
        currentState = MOVING_DOWN;
        lastDirection = MOVING_DOWN;
        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:
      break;
  }
  handleLimitSwitches();
}

// ================== Fungsi Tambahan ==================
void calibrateLift() {
  Serial.println("Memulai kalibrasi/homing...");
  unsigned long startTime = millis();
  currentState = CALIBRATING;

  if (digitalRead(limitPins[0]) == LOW) {
    currentFloor = 1;
    Serial.println("Kalibrasi OK. Lift sudah berada di lantai 1.");
    return;
  }

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

  unsigned long animationStartTime = millis();
  while (digitalRead(limitPins[0]) == HIGH && millis() - startTime < 10000) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(0, 29);
    display.println("Homing to Reference");
    display.setCursor(0, 39);
    display.print("Mohon tunggu...");
    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();

  if (digitalRead(limitPins[0]) == LOW) {
    currentFloor = 1;
    Serial.println("Kalibrasi Berhasil. Posisi: Lantai 1");
  } else {
    currentState = CALIBRATION_FAILED;
    Serial.println("Kalibrasi GAGAL! Periksa kabel atau limit switch.");
  }
}

void emergencyReset() {
  Serial.println("Lift mengalami keadaan darurat.");
  Serial.println("Memulai kalibrasi/homing otomatis ke Lantai 1...");
  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 > 15) totalFloors = 3;
  Serial.print("Total Floors: ");
  Serial.println(totalFloors);
}

void stopMotor() {
  digitalWrite(RELAY_MOTOR_UP, LOW);
  digitalWrite(RELAY_MOTOR_DOWN, LOW);
  delay(RELAY_SETTLE_MS);
  digitalWrite(RELAY_MOTOR_BRAKE, HIGH);
}

void controlMotor(ElevatorState state) {
  if (state == MOVING_UP && currentFloor >= totalFloors) {
    Serial.println("DI ATAS BATAS! Menghentikan motor.");
    stopMotor();
    currentState = IDLE;
    return;
  }
  if (state == MOVING_DOWN && currentFloor <= 1) {
    Serial.println("DI BAWAH BATAS! Menghentikan motor.");
    stopMotor();
    currentState = IDLE;
    return;
  }

  if (state == MOVING_UP) {
    if (lastDirection == MOVING_DOWN) {
      stopMotor();
      delay(MOTOR_DIRECTION_SWITCH_DELAY_MS);
    }
    digitalWrite(RELAY_MOTOR_UP, HIGH);
    digitalWrite(RELAY_MOTOR_BRAKE, LOW);
  } else if (state == MOVING_DOWN) {
    if (lastDirection == MOVING_UP) {
      stopMotor();
      delay(MOTOR_DIRECTION_SWITCH_DELAY_MS);
    }
    digitalWrite(RELAY_MOTOR_DOWN, HIGH);
    digitalWrite(RELAY_MOTOR_BRAKE, LOW);
  }
}

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;
  }

  // Tampilan status normal (non-darurat)
  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_OPEN || currentState == DOOR_CLOSING) {
    display.print("DOOR: ");
    switch (currentState) {
      case DOOR_OPENING:
        display.println("OPENING");
        break;
      case DOOR_OPEN: {
        unsigned long elapsed = millis() - doorTimer;
        int remaining = (DOOR_OPEN_HOLD_TIME - elapsed) / 1000;
        if (remaining < 0) remaining = 0;
        display.print("OPEN, Rem:(");
        display.print(remaining);
        display.print("s)");
        break;
      }
      case DOOR_CLOSING:
        display.println("CLOSING");
        break;
      default:
        break;
    }
  } 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]) {
      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 processCallButtons() {
  for (int i = 0; i < totalFloors; i++) {
    if (buttonPins[i] == 0) continue;

    bool reading = digitalRead(buttonPins[i]);
    unsigned long now = millis();

    if (reading == LOW && buttonState[i] == HIGH && (now - lastButtonPressTime[i]) > debounceDelay) {
      if (i + 1 == currentFloor) {
        if (currentState == IDLE || currentState == DOOR_OPEN || currentState == DOOR_OPENING) {
          Serial.println("Tombol lantai saat ini ditekan. Memicu pembukaan pintu.");
          currentState = DOOR_OPENING;
          doorTimer = now;
        }
      } else {
        bool isMovingUp = (currentState == MOVING_UP || (currentState == IDLE && (i + 1) > currentFloor));
        bool isMovingDown = (currentState == MOVING_DOWN || (currentState == IDLE && (i + 1) < currentFloor));
        if (isMovingUp) {
          if (i + 1 > currentFloor && !callUp[i]) {
            callUp[i] = true;
            Serial.print("Panggilan ke Lantai ");
            Serial.print(i + 1);
            Serial.println(" ditambahkan ke antrean naik.");
          }
        } else if (isMovingDown) {
          if (i + 1 < currentFloor && !callDown[i]) {
            callDown[i] = true;
            Serial.print("Panggilan ke Lantai ");
            Serial.print(i + 1);
            Serial.println(" ditambahkan ke antrean turun.");
          }
        }
      }
    }
    if (reading == LOW && buttonState[i] == HIGH) {
      lastButtonPressTime[i] = now;
    }
    buttonState[i] = reading;
  }
}

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

void handleLimitSwitches() {
  for (int i = 0; i < totalFloors; i++) {
    if (limitPins[i] == 0) continue;

    bool state = digitalRead(limitPins[i]);

    if (lastLimitState[i] == HIGH && state == LOW) {
      currentFloor = i + 1;
      Serial.print("Lift mencapai lantai ");
      Serial.println(currentFloor);

      if (callUp[i] || callDown[i] || (currentFloor == 1 && lastDirection == MOVING_DOWN) || (currentFloor == totalFloors && lastDirection == MOVING_UP)) {
        stopMotor();
        callUp[i] = false;
        callDown[i] = false;
        currentState = DOOR_OPENING;
        doorTimer = millis();
      }
    }
    lastLimitState[i] = state;
  }
}

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);
}