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

// --- Definisi Pin dan Konstanta ---
const int DIP_PIN_0 = 13;
const int DIP_PIN_1 = 14;
const int DIP_PIN_2 = 15;
const int DIP_PIN_3 = 27;

const int TEST_MODE_PIN = 35;
const int EMERGENCY_PIN = 23;

// FINAL LIMIT SWITCH PINS (PENTING: MENGGUNAKAN INTERUPSI)
const int FINAL_LIMIT_UP_PIN = 39;
const int FINAL_LIMIT_DOWN_PIN = 36;

// Komunikasi serial 2 arah untuk pintu
const int SERIAL_DOOR_RX = 16;
const int SERIAL_DOOR_TX = 17;

// Komunikasi serial untuk notifikasi audio lantai
const int SERIAL_AUDIO_RX = 9;
const int SERIAL_AUDIO_TX = 10;

// Shift Register Input
const int SR_IN_LATCH = 32;
const int SR_IN_CLOCK = 33;
const int SR_IN_DATA = 34;

// Shift Register Output
const int SR_OUT_LATCH = 25;
const int SR_OUT_CLOCK = 26;
const int SR_OUT_DATA = 5;

// Pin relay untuk motor
const int RELAY_MOTOR_UP = 19;
const int RELAY_MOTOR_DOWN = 18;
const int RELAY_MOTOR_BRAKE = 4;

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

// Durasi dan Timeout
const unsigned long DOOR_OPEN_DURATION_MS = 3000;
const unsigned long DOOR_CLOSE_DURATION_MS = 2000;
const unsigned long DOOR_OPEN_HOLD_TIME_MS = 5000;
const unsigned long MOTOR_TIMEOUT_MS = 15000;
const unsigned long MOTOR_DIRECTION_SWITCH_DELAY_MS = 500;
const unsigned long DEBOUNCE_DELAY_MS = 50;
const unsigned long LIMIT_DEBOUNCE_DELAY_MS = 100;
const unsigned long SERIAL_ACTIVE_RESET_DELAY_MS = 5000;

// --- Definisi Bit Shift Register Input 1 ---
const int COP_BUTTON_1_BIT = 0;
const int COP_BUTTON_2_BIT = 1;
const int EXT_BUTTON_UP_BIT = 2; // Panggilan Naik (LOP)
const int EXT_BUTTON_DOWN_BIT = 3; // Panggilan Turun (LOP)
const int DOOR_OPEN_BUTTON_BIT = 4;
const int DOOR_CLOSE_BUTTON_BIT = 5;
const int INSP_UP_BUTTON_BIT = 6;
const int INSP_DOWN_BUTTON_BIT = 7;

// --- Definisi Bit Shift Register Input 2 (Lantai 1-8) ---
const int LIMIT_FLOOR_1_BIT = 0;
const int LIMIT_FLOOR_2_BIT = 1;
const int LIMIT_FLOOR_3_BIT = 2;
const int LIMIT_FLOOR_4_BIT = 3;
const int LIMIT_FLOOR_5_BIT = 4;
const int LIMIT_FLOOR_6_BIT = 5;
const int LIMIT_FLOOR_7_BIT = 6;
const int LIMIT_FLOOR_8_BIT = 7;

// --- Definisi Bit Shift Register Input 3 (Lantai 9-15 & Lain-lain) ---
const int LIMIT_FLOOR_9_BIT = 0;
const int LIMIT_FLOOR_10_BIT = 1;
const int LIMIT_FLOOR_11_BIT = 2;
const int LIMIT_FLOOR_12_BIT = 3;
const int LIMIT_FLOOR_13_BIT = 4;
const int LIMIT_FLOOR_14_BIT = 5;
const int LIMIT_FLOOR_15_BIT = 6;
const int CABIN_LIGHT_BUTTON_BIT = 7;

// --- Definisi Bit Shift Register Output 1 (Lampu Status) ---
const int ERROR_LED_BIT = 0;
const int SYSTEM_OK_LED_BIT = 1;
const int COP_CALL_LED_BIT = 2;
const int EXTERNAL_CALL_LED_BIT = 3;
const int SERIAL_COM_OK_LED_BIT = 4;
const int CABIN_LIGHT_OUTPUT_BIT = 5;
const int DUMMY_BIT_2 = 6;
const int DUMMY_BIT_3 = 7;

// --- Definisi Bit Shift Register Output 2 (Buzzer & lain-lain) ---
const int BUZZER_BIT = 0;

// --- Variabel Global ---
int totalFloors = 3;
int currentFloor = 0;
bool cabinLightOn = false;
unsigned long lastSerialDataTime = 0;

// Variabel volatile, dimodifikasi oleh ISRs.
volatile bool finalLimitUpTriggered = false;
volatile bool finalLimitDownTriggered = false;

byte inputData1, inputData2, inputData3;
byte outputData1, outputData2;

bool callUp[16] = {false};
bool callDown[16] = {false};
bool copCallActive[16] = {false};

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

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

bool isEmergency = false;
unsigned long stateTimer = 0;
unsigned long motorStartTime = 0;
unsigned long lastButtonPressTime[40] = {0}; 
unsigned long lastLimitStatusChange[16] = {0};
bool lastLimitStatus[16];
unsigned long lastMotorDirectionSwitchTime = 0;

// Variabel untuk display dan UI
unsigned long lastBlink = 0;
bool blinkState = true;
unsigned long previousMillis = 0;
int dotCount = 0;

// --- Prototipe Fungsi ---
void readDipSwitch();
void controlMotor(ElevatorState state);
void stopMotor();
void displayStatus();
void displayIdleMoving();
void displayDoorStatus();
void displayEmergency();
void displayCalibration();
void displayCallQueue();
void processCallButtons();
void handleLimitSwitches();
void handleEmergency();
void emergencyReset();
int findNextFloor();
void handleModeChange();
void handleNormalMode();
void handleTestMode();
void handleInspectionMode();
void handleCalibrationState();
void drawArrowUp(int x, int y, int size);
void drawArrowDown(int x, int y, int size);
void readAllSRInputs();
void writeAllSROutputs();
void updateFloorLeds();
void processDoorAndLightButtons();
void handleMotorTimeout();
int decodeDip(int val);
void processExternalCalls();
void IRAM_ATTR handleFinalLimitUp();
void IRAM_ATTR handleFinalLimitDown();
void showStartup();
bool checkForCalls();
bool isTargetFloor(int floor);

// --- ISRs UNTUK FINAL LIMIT SWITCH ---
// Catatan: ISR harus sesingkat mungkin.
void IRAM_ATTR handleFinalLimitUp() {
  finalLimitUpTriggered = true;
}

void IRAM_ATTR handleFinalLimitDown() {
  finalLimitDownTriggered = true;
}

// =================================== SETUP ===================================
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(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, SERIAL_AUDIO_RX, SERIAL_AUDIO_TX);

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

  readDipSwitch();
  showStartup();

  pinMode(FINAL_LIMIT_UP_PIN, INPUT_PULLUP);
  pinMode(FINAL_LIMIT_DOWN_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(FINAL_LIMIT_UP_PIN), handleFinalLimitUp, FALLING);
  attachInterrupt(digitalPinToInterrupt(FINAL_LIMIT_DOWN_PIN), handleFinalLimitDown, FALLING);

  readAllSRInputs();
  for (int i = 0; i < totalFloors; i++) {
    bool limitStatus = (i < 8) ? (bitRead(inputData2, i) == 0) : (bitRead(inputData3, i - 8) == 0);
    lastLimitStatus[i] = limitStatus;
  }
  
  // Mengatur state awal untuk memulai kalibrasi di loop()
  currentState = CALIBRATING;
  motorStartTime = millis();
}

// =================================== LOOP UTAMA ===================================
void loop() {
  readAllSRInputs();
  writeAllSROutputs();
  handleEmergency();
  
  if (millis() - lastSerialDataTime > SERIAL_ACTIVE_RESET_DELAY_MS) {
    bitClear(outputData1, SERIAL_COM_OK_LED_BIT);
  } else {
    bitSet(outputData1, SERIAL_COM_OK_LED_BIT);
  }

  // --- Penanganan Final Limit Switch dengan Pencegahan Race Condition ---
  bool tempFinalLimitUp, tempFinalLimitDown;
  noInterrupts();
  tempFinalLimitUp = finalLimitUpTriggered;
  tempFinalLimitDown = finalLimitDownTriggered;
  finalLimitUpTriggered = false;
  finalLimitDownTriggered = false;
  interrupts();
  
  if (tempFinalLimitUp || tempFinalLimitDown) {
    if (!isEmergency) {
      Serial.println("FINAL LIMIT SWITCH TERPICU! Lift dihentikan karena over-travel.");
      isEmergency = true;
      currentState = EMERGENCY_STOP;
      stopMotor();
    }
  }

  if (isEmergency || currentState == CALIBRATION_FAILED) {
    displayStatus();
    updateFloorLeds();
    return;
  }
  
  // Penanganan state
  switch (currentState) {
    case CALIBRATING:
      handleCalibrationState();
      break;
    case EMERGENCY_RESETTING:
      emergencyReset();
      break;
    default:
      handleModeChange();
      switch (currentMode) {
        case MODE_NORMAL:
          handleNormalMode();
          break;
        case MODE_TEST:
          handleTestMode();
          break;
        case MODE_INSPECTION:
          handleInspectionMode();
          break;
      }
  }

  updateFloorLeds();
  displayStatus();
}

// ================== Fungsi Mode Operasi & Kalibrasi ==================
void handleCalibrationState() {
  // Catatan: Logika ini mengasumsikan lift akan bergerak ke bawah sampai menemukan limit switch di lantai 1.
  bool limitFloor1 = (bitRead(inputData2, LIMIT_FLOOR_1_BIT) == 0);
  if (limitFloor1) {
    stopMotor();
    currentFloor = 1;
    currentState = IDLE;
    Serial.println("Kalibrasi Berhasil. Posisi: Lantai 1");
  } else {
    controlMotor(MOVING_DOWN); // Selalu bergerak ke bawah untuk homing
    
    if (millis() - motorStartTime > MOTOR_TIMEOUT_MS) {
      stopMotor();
      currentState = CALIBRATION_FAILED;
      Serial.println("Kalibrasi GAGAL! Periksa kabel atau limit switch.");
    }
    
    // Periksa final limit switch di sini juga sebagai lapisan proteksi
    bool tempFinalLimitDown;
    noInterrupts();
    tempFinalLimitDown = finalLimitDownTriggered;
    interrupts();
    if (tempFinalLimitDown) {
      stopMotor();
      currentState = CALIBRATION_FAILED;
      Serial.println("Final limit switch bawah terpicu saat kalibrasi. Kalibrasi gagal.");
      noInterrupts();
      finalLimitDownTriggered = false;
      interrupts();
    }
  }
}

void emergencyReset() {
  Serial.println("Lift mengalami keadaan darurat.");
  Serial.println("Memulai kalibrasi/homing otomatis ke Lantai 1...");
  isEmergency = false;
  currentState = CALIBRATING; // Cukup ubah state, loop() akan menangani sisanya
  motorStartTime = millis();
}

void handleModeChange() {
  bool currentTestModePinState = digitalRead(TEST_MODE_PIN) == LOW;
  unsigned long now = millis();

  if (currentTestModePinState && (now - lastButtonPressTime[TEST_MODE_PIN] > DEBOUNCE_DELAY_MS)) {
    stopMotor();
    for (int i = 0; i < totalFloors; i++) {
      callUp[i] = false;
      callDown[i] = false;
      copCallActive[i] = false;
    }

    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;
    }
    currentState = IDLE;
    lastButtonPressTime[TEST_MODE_PIN] = now;
  }
}

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

  switch (currentState) {
    case IDLE: {
      int nextFloor = findNextFloor();
      if (nextFloor != -1) {
        if (nextFloor > currentFloor) {
          currentState = MOVING_UP;
        } else {
          currentState = MOVING_DOWN;
        }
        motorStartTime = millis();
      }
      break;
    }
    case MOVING_UP:
    case MOVING_DOWN:
      controlMotor(currentState);
      break;
    case STOPPING:
        stopMotor();
        if (checkForCalls() && (millis() - stateTimer > MOTOR_DIRECTION_SWITCH_DELAY_MS)) {
            currentState = DOOR_OPENING;
            stateTimer = millis();
        } else {
            currentState = IDLE;
        }
        break;
    case DOOR_OPENING:
      // Catatan: Kode ini mengirim perintah serial ke modul pintu eksternal.
      // Modul tersebut harus diprogram untuk merespons perintah "OPEN\n" dan "CLOSE\n".
      Serial2.print("OPEN\n");
      if (millis() - stateTimer > DOOR_OPEN_DURATION_MS) {
        currentState = DOOR_OPEN;
        stateTimer = millis();
      }
      break;
    case DOOR_OPEN:
      if (millis() - stateTimer > DOOR_OPEN_HOLD_TIME_MS) {
        currentState = DOOR_CLOSING;
        stateTimer = millis();
      }
      break;
    case DOOR_CLOSING: {
      Serial2.print("CLOSE\n");
      if (millis() - stateTimer > DOOR_CLOSE_DURATION_MS) {
        int nextFloor = findNextFloor();
        if (nextFloor != -1) {
          if (nextFloor > currentFloor) {
            currentState = MOVING_UP;
          } else {
            currentState = MOVING_DOWN;
          }
          motorStartTime = millis();
        } else {
          currentState = IDLE;
        }
      }
      break;
    }
    default:
      break;
  }
}

void handleTestMode() {
  handleNormalMode();
}

void handleInspectionMode() {
  unsigned long now = millis();
  bool inspUpState = (bitRead(inputData1, INSP_UP_BUTTON_BIT) == 0);
  bool inspDownState = (bitRead(inputData1, INSP_DOWN_BUTTON_BIT) == 0);

  if (currentState == MOVING_UP || currentState == MOVING_DOWN) {
      if (!inspUpState && !inspDownState) {
          stopMotor();
          currentState = IDLE;
          return;
      }
  }

  if (inspUpState && (now - lastButtonPressTime[INSP_UP_BUTTON_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentState == IDLE || currentState == STOPPING) {
      currentState = MOVING_UP;
      lastDirection = MOVING_UP;
      motorStartTime = now;
    }
    lastButtonPressTime[INSP_UP_BUTTON_BIT] = now;
  }

  if (inspDownState && (now - lastButtonPressTime[INSP_DOWN_BUTTON_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentState == IDLE || currentState == STOPPING) {
      currentState = MOVING_DOWN;
      lastDirection = MOVING_DOWN;
      motorStartTime = now;
    }
    lastButtonPressTime[INSP_DOWN_BUTTON_BIT] = now;
  }

  if(currentState == MOVING_UP || currentState == MOVING_DOWN) {
    controlMotor(currentState);
  } else {
      stopMotor();
  }

  handleLimitSwitches();
  handleMotorTimeout();
}

// =================================== Fungsi Tambahan ===================================
int decodeDip(int val) {
  switch (val) {
    case 0b1101: return 2;
    case 0b1100: return 3;
    case 0b1011: return 4;
    case 0b1010: return 5;
    case 0b1001: return 6;
    case 0b1000: return 7;
    case 0b0111: return 8;
    case 0b0110: return 9;
    case 0b0101: return 10;
    case 0b0100: return 11;
    case 0b0011: return 12;
    case 0b0010: return 13;
    case 0b0001: return 14;
    case 0b0000: return 15;
    default: return 3;
  }
}

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 = decodeDip(dipValue);
  Serial.print("Total Floors: ");
  Serial.println(totalFloors);
}

void stopMotor() {
  digitalWrite(RELAY_MOTOR_UP, LOW);
  digitalWrite(RELAY_MOTOR_DOWN, LOW);
  unsigned long brakeTimer = millis();
  while (millis() - brakeTimer < 30);
  digitalWrite(RELAY_MOTOR_BRAKE, HIGH);
}

// FUNGSI INI SUDAH DIKEMBANGKAN DENGAN LOGIKA NON-BLOCKING
void controlMotor(ElevatorState state) {
    unsigned long now = millis();
    
    // Logic to prevent motor from starting until switch delay is passed
    if (now - lastMotorDirectionSwitchTime < MOTOR_DIRECTION_SWITCH_DELAY_MS) {
        return;
    }

    if (state == MOVING_UP) {
        if (lastDirection != MOVING_UP) {
            stopMotor();
            lastMotorDirectionSwitchTime = now;
            // Return early to wait for the non-blocking delay
            return;
        }
        digitalWrite(RELAY_MOTOR_UP, HIGH);
        digitalWrite(RELAY_MOTOR_DOWN, LOW);
        digitalWrite(RELAY_MOTOR_BRAKE, LOW);
        lastDirection = MOVING_UP;
    } else if (state == MOVING_DOWN) {
        if (lastDirection != MOVING_DOWN) {
            stopMotor();
            lastMotorDirectionSwitchTime = now;
            // Return early to wait for the non-blocking delay
            return;
        }
        digitalWrite(RELAY_MOTOR_DOWN, HIGH);
        digitalWrite(RELAY_MOTOR_UP, LOW);
        digitalWrite(RELAY_MOTOR_BRAKE, LOW);
        lastDirection = MOVING_DOWN;
    } else {
        stopMotor();
    }
}

void displayStatus() {
  display.clearDisplay();
  if (currentState == CALIBRATION_FAILED) {
    displayCalibration();
  } else if (currentState == CALIBRATING) {
    displayCalibration();
  } else if (currentState == EMERGENCY_STOP) {
    displayEmergency();
  } else if (currentState == DOOR_OPENING || currentState == DOOR_CLOSING || currentState == DOOR_OPEN) {
    displayDoorStatus();
  } else {
    displayIdleMoving();
  }
  display.display();
}

void displayIdleMoving() {
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Orch.Gen 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);
  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);
      }
    }
  }
}

void displayDoorStatus() {
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Orch.Gen Elevator");
  display.drawLine(0, 9, SCREEN_WIDTH - 1, 9, WHITE);

  display.setCursor(0, 57);
  display.print("DOOR: ");
  if (currentState == DOOR_OPENING) {
    display.print("OPENING");
  } else if (currentState == DOOR_CLOSING) {
    display.print("CLOSING");
  } else if (currentState == DOOR_OPEN) {
    display.print("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)");
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= 500) {
    previousMillis = currentMillis;
    dotCount = (dotCount + 1) % 4;
  }
  if (currentState != DOOR_OPEN) {
      for (int i = 0; i < dotCount; i++) {
        display.print(".");
      }
  }
}

void displayEmergency() {
  display.setCursor(10, 20);
  display.setTextSize(2);
  if (blinkState) {
    display.println("EMERGENCY");
  } else {
    display.println("STOPPED");
  }
  if (millis() - lastBlink > 500) {
    lastBlink = millis();
    blinkState = !blinkState;
  }
}

void displayCalibration() {
  display.setTextSize(1);
  display.setCursor(5, 0);
  if (currentState == CALIBRATION_FAILED) {
      display.println("--- HOMING FAILED ---");
  } else {
      display.println("--- HOMING ---");
  }
  
  display.setTextSize(1);
  display.setCursor(0, 40);
  display.print("Mohon tunggu...");
  
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= 500) {
    previousMillis = currentMillis;
    dotCount = (dotCount + 1) % 4;
  }
  for (int i = 0; i < dotCount; i++) {
    display.print(".");
  }

  unsigned long elapsedTime = millis() - motorStartTime;
  int barWidth = map(elapsedTime, 0, MOTOR_TIMEOUT_MS, 0, SCREEN_WIDTH);
  display.drawRect(0, 50, SCREEN_WIDTH - 1, 5, WHITE);
  display.fillRect(0, 51, barWidth, 3, WHITE);
}

void displayCallQueue() {
  int x = 47;
  int y = 47;
  int callCount = 0;
  bool anyCall = false;

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

void processExternalCalls() {
  // Logika ini tetap ada untuk menerima panggilan dari komunikasi serial
  if (Serial2.available() > 0) {
    lastSerialDataTime = millis();
    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;
        } else if (floor < currentFloor) {
          callDown[floor - 1] = true;
        } else {
          if (currentState == IDLE || currentState == DOOR_OPEN) {
            currentState = DOOR_OPENING;
            stateTimer = millis();
          }
        }
      }
    }
  }
}

void processDoorAndLightButtons() {
  unsigned long now = millis();
  bool openButton = (bitRead(inputData1, DOOR_OPEN_BUTTON_BIT) == 0);
  bool closeButton = (bitRead(inputData1, DOOR_CLOSE_BUTTON_BIT) == 0);
  bool cabinLightButton = (bitRead(inputData3, CABIN_LIGHT_BUTTON_BIT) == 0);

  if (openButton && (now - lastButtonPressTime[DOOR_OPEN_BUTTON_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentState == DOOR_CLOSING || currentState == IDLE || currentState == DOOR_OPEN) {
      currentState = DOOR_OPENING;
      stateTimer = now;
    }
    lastButtonPressTime[DOOR_OPEN_BUTTON_BIT] = now;
  }

  if (closeButton && (now - lastButtonPressTime[DOOR_CLOSE_BUTTON_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentState == DOOR_OPEN || currentState == IDLE) {
      currentState = DOOR_CLOSING;
      stateTimer = now;
    }
    lastButtonPressTime[DOOR_CLOSE_BUTTON_BIT] = now;
  }

  if (cabinLightButton && (now - lastButtonPressTime[CABIN_LIGHT_BUTTON_BIT] > DEBOUNCE_DELAY_MS)) {
    cabinLightOn = !cabinLightOn;
    lastButtonPressTime[CABIN_LIGHT_BUTTON_BIT] = now;
  }
}

void processCallButtons() {
  unsigned long now = millis();
  bool cop1Button = (bitRead(inputData1, COP_BUTTON_1_BIT) == 0);
  bool cop2Button = (bitRead(inputData1, COP_BUTTON_2_BIT) == 0);
  bool lopUpState = (bitRead(inputData1, EXT_BUTTON_UP_BIT) == 0);
  bool lopDownState = (bitRead(inputData1, EXT_BUTTON_DOWN_BIT) == 0);

  // Proses tombol COP
  if (cop1Button && (now - lastButtonPressTime[COP_BUTTON_1_BIT] > DEBOUNCE_DELAY_MS)) {
    copCallActive[0] = true;
    lastButtonPressTime[COP_BUTTON_1_BIT] = now;
  }
  
  if (cop2Button && (now - lastButtonPressTime[COP_BUTTON_2_BIT] > DEBOUNCE_DELAY_MS)) {
    copCallActive[1] = true;
    lastButtonPressTime[COP_BUTTON_2_BIT] = now;
  }

  // Proses tombol LOP
  if (lopUpState && (now - lastButtonPressTime[EXT_BUTTON_UP_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentFloor < totalFloors) {
      callUp[currentFloor] = true;
      Serial.print("Panggilan LOP Naik dari Lantai ");
      Serial.print(currentFloor + 1);
      Serial.println(" ditambahkan ke antrean.");
    }
    lastButtonPressTime[EXT_BUTTON_UP_BIT] = now;
  }

  if (lopDownState && (now - lastButtonPressTime[EXT_BUTTON_DOWN_BIT] > DEBOUNCE_DELAY_MS)) {
    if (currentFloor > 1) {
      callDown[currentFloor - 2] = true;
      Serial.print("Panggilan LOP Turun dari Lantai ");
      Serial.print(currentFloor + 1);
      Serial.println(" ditambahkan ke antrean.");
    }
    lastButtonPressTime[EXT_BUTTON_DOWN_BIT] = now;
  }
}

int findNextFloor() {
  // Jika lift sedang naik atau idle (arah default naik)
  if (lastDirection == MOVING_UP) {
    for (int i = currentFloor; i < totalFloors; i++) {
      if (copCallActive[i] || callUp[i]) {
        return i + 1;
      }
    }
    // Jika tidak ada panggilan di atas, cari panggilan turun di bawah
    for (int i = totalFloors - 1; i >= 0; i--) {
      if (copCallActive[i] || callDown[i]) {
        lastDirection = MOVING_DOWN;
        return i + 1;
      }
    }
  } 
  // Jika lift sedang turun atau idle (arah default turun)
  else { // lastDirection == MOVING_DOWN
    for (int i = currentFloor - 2; i >= 0; i--) {
      if (copCallActive[i] || callDown[i]) {
        return i + 1;
      }
    }
    // Jika tidak ada panggilan di bawah, cari panggilan naik di atas
    for (int i = 0; i < totalFloors; i++) {
      if (copCallActive[i] || callUp[i]) {
        lastDirection = MOVING_UP;
        return i + 1;
      }
    }
  }
  return -1; // Tidak ada panggilan aktif
}

bool checkForCalls() {
  for (int i = 0; i < totalFloors; i++) {
    if (copCallActive[i] || callUp[i] || callDown[i]) {
      return true;
    }
  }
  return false;
}

void handleLimitSwitches() {
  unsigned long now = millis();
  
  for (int i = 0; i < totalFloors; i++) {
    bool currentLimitStatus = false;
    if (i < 8) {
      currentLimitStatus = (bitRead(inputData2, i) == 0);
    } else {
      currentLimitStatus = (bitRead(inputData3, i - 8) == 0);
    }

    if (currentLimitStatus && !lastLimitStatus[i] && (now - lastLimitStatusChange[i] > LIMIT_DEBOUNCE_DELAY_MS)) {
      currentFloor = i + 1;
      Serial.print("Limit switch terpicu di lantai ");
      Serial.println(currentFloor);
      Serial1.print(currentFloor);
      Serial1.print("\n");
      
      if (isTargetFloor(currentFloor)) {
          stopMotor();
          callUp[currentFloor-1] = false;
          callDown[currentFloor-1] = false;
          copCallActive[currentFloor-1] = false;
          currentState = STOPPING;
          stateTimer = now;
          Serial.println("Mencapai lantai tujuan. Pintu akan terbuka.");
      }
      
      lastLimitStatus[i] = currentLimitStatus;
      lastLimitStatusChange[i] = now;
    } else if (!currentLimitStatus && lastLimitStatus[i]) {
        if (now - lastLimitStatusChange[i] > LIMIT_DEBOUNCE_DELAY_MS) {
            lastLimitStatus[i] = currentLimitStatus;
            lastLimitStatusChange[i] = now;
        }
    }
  }
}

bool isTargetFloor(int floor) {
  if (copCallActive[floor - 1]) return true;
  if (lastDirection == MOVING_UP && callUp[floor - 1]) return true;
  if (lastDirection == MOVING_DOWN && callDown[floor - 1]) return true;
  return false;
}

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 && (millis() - lastButtonPressTime[EMERGENCY_PIN] > DEBOUNCE_DELAY_MS)) {
    isEmergency = true;
    currentState = EMERGENCY_STOP;
    stopMotor();
    Serial.println("Tombol darurat ditekan!");
    lastButtonPressTime[EMERGENCY_PIN] = millis();
  } else if (!emergencyPinState && isEmergency && (millis() - lastButtonPressTime[EMERGENCY_PIN] > 1000)) { // Tambah debounce untuk reset
    isEmergency = false;
    currentState = EMERGENCY_RESETTING;
    Serial.println("Reset darurat.");
    lastButtonPressTime[EMERGENCY_PIN] = millis();
  }
}

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

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

void showStartup() {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("  Orch.Gen");
  display.setCursor(0, 20);
  display.print("   ELEVATOR");
  display.setTextSize(1);
  display.setCursor(15, 50);
  display.print("Initializing...");
  display.display();
  unsigned long start = millis();
  while (millis() - start < 3000) {}
}

void readAllSRInputs() {
  digitalWrite(SR_IN_LATCH, LOW);
  delayMicroseconds(5);
  digitalWrite(SR_IN_LATCH, HIGH);
  inputData3 = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
  inputData2 = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
  inputData1 = shiftIn(SR_IN_DATA, SR_IN_CLOCK, MSBFIRST);
}

void writeAllSROutputs() {
  digitalWrite(SR_OUT_LATCH, LOW);
  shiftOut(SR_OUT_DATA, SR_OUT_CLOCK, MSBFIRST, outputData2);
  shiftOut(SR_OUT_DATA, SR_OUT_CLOCK, MSBFIRST, outputData1);
  digitalWrite(SR_OUT_LATCH, HIGH);
}

void updateFloorLeds() {
  outputData1 = 0;
  outputData2 = 0;

  bool anyCopCall = false;
  for (int i = 0; i < totalFloors; i++) {
    if (copCallActive[i]) {
      anyCopCall = true;
      break;
    }
  }
  if (anyCopCall) {
    bitSet(outputData1, COP_CALL_LED_BIT);
  } else {
    bitClear(outputData1, COP_CALL_LED_BIT);
  }

  bool anyExtCall = false;
  for (int i = 0; i < totalFloors; i++) {
    if (callUp[i] || callDown[i]) {
      anyExtCall = true;
      break;
    }
  }
  if (anyExtCall) {
    bitSet(outputData1, EXTERNAL_CALL_LED_BIT);
  } else {
    bitClear(outputData1, EXTERNAL_CALL_LED_BIT);
  }

  if (millis() - lastSerialDataTime < SERIAL_ACTIVE_RESET_DELAY_MS) {
    bitSet(outputData1, SERIAL_COM_OK_LED_BIT);
  } else {
    bitClear(outputData1, SERIAL_COM_OK_LED_BIT);
  }

  if (isEmergency || currentState == CALIBRATION_FAILED) {
    bitSet(outputData1, ERROR_LED_BIT);
    bitSet(outputData2, BUZZER_BIT);
  } else {
    bitClear(outputData1, ERROR_LED_BIT);
    bitClear(outputData2, BUZZER_BIT);
  }
  
  if (isEmergency || currentState == CALIBRATION_FAILED) {
    bitClear(outputData1, SYSTEM_OK_LED_BIT);
  } else {
    bitSet(outputData1, SYSTEM_OK_LED_BIT);
  }

  if (cabinLightOn) {
    bitSet(outputData1, CABIN_LIGHT_OUTPUT_BIT);
  } else {
    bitClear(outputData1, CABIN_LIGHT_OUTPUT_BIT);
  }
}