#include "config.h"
#include "motor.h"
#include "display.h"
#include "serial_comm.h"

// Prototipe fungsi
void handleFinalLimits();
void handleModeChange();
void handleNormalMode();
void handleTestMode();
void handleInspectionMode();
void processCopCalls();
void processLopCalls();
void processDoorAndLightButtons();
void handleLimitSwitches();
void handleEmergency();
void emergencyReset();
int findNextFloor();
void controlMotor(ElevatorState direction);
void calibrateLift();
void stopMotor();
void readAllSRInputs();
void writeAllSROutputs();
void showStartup();
void displayStatus();
void readDipSwitch();
void handleMotorTimeout();

// Inisialisasi variabel global (definisi)
int totalFloors = 3;
int currentFloor = 0;
bool cabinLightOn = false;
byte inputData1 = 0, inputData2 = 0;
byte outputData1 = 0, outputData2 = 0;
bool callUp[16] = {false};
bool callDown[16] = {false};
bool copCallActive[16] = {false};
ElevatorState currentState = CALIBRATING;
ElevatorState lastDirection = MOVING_UP;
OperationMode currentMode = MODE_NORMAL;
bool isEmergency = false;
unsigned long stateTimer = 0;
unsigned long motorStartTime = 0;

// Variabel debounce dan timing
unsigned long lastButtonPressTime[16] = {0};
bool buttonState[16] = {0};
bool lastTestModePinState = HIGH;
unsigned long lastModeChangeTime = 0;
bool lastCabinLightButtonState = HIGH;
unsigned long lastCabinLightTime = 0;
bool lastLimitStatus[16] = {0};
unsigned long lastLimitDebounceTime[16] = {0};
unsigned long lastCallUpTime = 0;
unsigned long lastCallDownTime = 0;
unsigned long lastActivityTime = 0;

// display timing
unsigned long lastBlink = 0;
bool blinkState = true;
unsigned long previousMillis = 0;
int dotCount = 0;

// --- Variabel global baru untuk lampu kabin ---
unsigned long lastCabinLightActivityTime = 0;

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

    // pins
    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);
    // Nonaktifkan sementara final limit
    // pinMode(FINAL_LIMIT_UP_PIN, INPUT_PULLUP);
    // pinMode(FINAL_LIMIT_DOWN_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();

    initSerial();

    updateFloorLeds();

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

    readAllSRInputs();
    for (int i = 0; i < totalFloors; i++) {
        lastLimitStatus[i] = (bitRead(inputData2, i) == 0);
    }
    showStartup();

    readDipSwitch();
    calibrateLift();

    if (currentState != CALIBRATION_FAILED) {
        currentState = IDLE;
        Serial.println("System is ready. State: IDLE");
        printTimestamp();
    }
    lastActivityTime = millis();
}

void loop() {
    handleSerialCommands();
    readAllSRInputs();
    handleEmergency();
    handleModeChange();
    // Nonaktifkan sementara pemantauan final limits
    // handleFinalLimits();

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

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

// ================== Fungsi Mode & Input Handling ==================
void handleModeChange() {
    bool currentTestModePinState = digitalRead(TEST_MODE_PIN);
    unsigned long now = millis();
    if (lastTestModePinState == HIGH && currentTestModePinState == LOW && (now - lastModeChangeTime) > DEBOUNCE_DELAY_MS) {
        switch (currentMode) {
            case MODE_NORMAL:
                currentMode = MODE_TEST;
                Serial.println("Mode changed to: TEST");
                printTimestamp();
                break;
            case MODE_TEST:
                currentMode = MODE_INSPECTION;
                Serial.println("Mode changed to: INSPECTION");
                printTimestamp();
                break;
            case MODE_INSPECTION:
                currentMode = MODE_NORMAL;
                Serial.println("Mode changed to: NORMAL");
                printTimestamp();
                break;
        }
        lastModeChangeTime = now;
        stopMotor();
        currentState = IDLE;
        for (int i = 0; i < totalFloors; i++) {
            callUp[i] = false;
            callDown[i] = false;
            copCallActive[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)");
                    printTimestamp();
                } else if (floor < currentFloor) {
                    callDown[floor - 1] = true;
                    Serial.print("Panggilan UART ke Lantai "); Serial.print(floor); Serial.println(" (TURUN)");
                    printTimestamp();
                } else {
                    if (currentState == IDLE || currentState == DOOR_OPEN) {
                        currentState = DOOR_OPENING;
                        stateTimer = millis();
                        Serial.println("Panggilan ke lantai saat ini. Memicu pembukaan pintu.");
                        printTimestamp();
                    }
                }
            }
        }
    }

    handleLimitSwitches();
    handleMotorTimeout();

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

    switch (currentState) {
        case IDLE: {
            lastActivityTime = millis();
            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 DOOR_OPENING:
            Serial2.print("OPEN\n");
            stopMotor(); 
            if (millis() - stateTimer > DOOR_OPEN_DURATION_MS) {
                currentState = DOOR_OPEN;
                stateTimer = millis();
            }
            break;
        case DOOR_OPEN:
            lastActivityTime = millis();
            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;
                        lastDirection = MOVING_UP;
                    } else {
                        currentState = MOVING_DOWN;
                        lastDirection = MOVING_DOWN;
                    }
                    motorStartTime = millis();
                } else {
                    currentState = IDLE;
                }
            }
            break;
        }
        case EMERGENCY_RESETTING:
            emergencyReset();
            break;
        default:
            break;
    }
}

void handleTestMode() {
    processCopCalls();
    processLopCalls();
    processDoorAndLightButtons();
    handleLimitSwitches();
    handleNormalMode();
}

void handleInspectionMode() {
    // Tombol INS UP dan INS DOWN
    bool insUpButton = (bitRead(inputData1, INS_UP_BUTTON_BIT) == 0);
    bool insDownButton = (bitRead(inputData1, INS_DOWN_BUTTON_BIT) == 0);

    // Prioritaskan tombol INS UP jika keduanya ditekan
    if (insUpButton) {
        if (currentState != MOVING_UP) {
            Serial.println("INSPECTION: Moving UP.");
            printTimestamp();
        }
        currentState = MOVING_UP;
        controlMotor(currentState);
    } else if (insDownButton) {
        if (currentState != MOVING_DOWN) {
            Serial.println("INSPECTION: Moving DOWN.");
            printTimestamp();
        }
        currentState = MOVING_DOWN;
        controlMotor(currentState);
    } else {
        if (currentState != IDLE) {
            Serial.println("INSPECTION: STOP.");
            printTimestamp();
        }
        stopMotor();
        currentState = IDLE;
    }
    // Periksa limit switches di setiap loop
    handleLimitSwitches();
    // handleMotorTimeout(); // Dinonaktifkan di mode inspeksi
}

// =================================== Fungsi Tambahan ===================================
/*
* @brief Menangani pemicuan sakelar batas akhir (final limit switches).
* Fungsi ini akan menghentikan motor dan memicu mode darurat.
*/
void handleFinalLimits() {
    // bool finalLimitUpActive = (digitalRead(FINAL_LIMIT_UP_PIN) == 0);
    // bool finalLimitDownActive = (digitalRead(FINAL_LIMIT_DOWN_PIN) == 0);
    // Deteksi pemicuan salah satu final limit switch
    // if (!isEmergency && ((finalLimitUpActive && currentState == MOVING_UP) || (finalLimitDownActive && currentState == MOVING_DOWN))) {
    // Serial.println("FINAL LIMIT SWITCH AKTIF! Lift dihentikan karena over-travel.");
    // isEmergency = true;
    // currentState = EMERGENCY_STOP;
    // stopMotor();
    // }
}

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

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

    // Reset aktivitas umum lift dan NYALAKAN lampu kabin bila tombol pintu ditekan
    if (openButton || closeButton) {
        lastActivityTime = now;
        cabinLightOn = true;
    }

    // Handle tombol pintu
    if (openButton && (currentState == IDLE || currentState == DOOR_CLOSING || currentState == DOOR_OPEN)) {
        currentState = DOOR_OPENING;
        stateTimer = now;
        return;
    }
    if (closeButton && (currentState == DOOR_OPEN || currentState == DOOR_OPENING)) {
        currentState = DOOR_CLOSING;
        stateTimer = now;
        return;
    }

    // Handle tombol lampu kabin
    bool currentCabinLightButtonState = (bitRead(inputData1, CABIN_LIGHT_BUTTON_BIT) == 0);
    static unsigned long lastCabinLightActivityTime = 0;

    if (lastCabinLightButtonState == HIGH && currentCabinLightButtonState == LOW && 
        (now - lastCabinLightTime) > DEBOUNCE_DELAY_MS) {
        
        cabinLightOn = !cabinLightOn;
        lastCabinLightTime = now;

        if (cabinLightOn) {
            lastCabinLightActivityTime = now; // reset timer saat dinyalakan
        }

        Serial.print("Lampu Kabin: ");
        Serial.println(cabinLightOn ? "ON" : "OFF");
        printTimestamp();
    }
    lastCabinLightButtonState = currentCabinLightButtonState;

    // Reset aktivitas lampu dan NYALAKAN lampu bila ada call di COP
    for (int i = 0; i < totalFloors; i++) {
        bool copCall = (bitRead(inputData1, i) == 0);
        if (copCall) {
            lastCabinLightActivityTime = now;  // hanya untuk lampu
            cabinLightOn = true;
        }
    }

    // Timeout lampu kabin
    if (cabinLightOn && (now - lastCabinLightActivityTime) > CABIN_LIGHT_TIMEOUT_MS) {
        cabinLightOn = false;
        Serial.println("Lampu kabin dimatikan karena timeout.");
        printTimestamp();
    }
}

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

  for (int i = 0; i < 3; i++) {
    bool reading = (bitRead(inputData1, i) == 0);
    if (reading && (now - lastButtonPressTime[i]) > DEBOUNCE_DELAY_MS) {
      lastActivityTime = now;
      if (i + 1 == currentFloor) {
        if (currentState == IDLE || currentState == DOOR_OPEN) {
          Serial.println("Tombol lantai saat ini ditekan. Memicu pembukaan pintu.");
          printTimestamp();
          currentState = DOOR_OPENING;
          stateTimer = now;
        }
      } else {
        if (i + 1 > currentFloor) {
          callUp[i] = true;
          copCallActive[i] = true;
          Serial.print("Panggilan COP ke Lantai ");
          Serial.print(i + 1);
          Serial.println(" ditambahkan ke antrean naik.");
          printTimestamp();
        } else if (i + 1 < currentFloor) {
          callDown[i] = true;
          copCallActive[i] = true;
          Serial.print("Panggilan COP ke Lantai ");
          Serial.print(i + 1);
          Serial.println(" ditambahkan ke antrean turun.");
          printTimestamp();
        }
      }
      lastButtonPressTime[i] = now;
    }
  }
}
    
void processLopCalls() {
  unsigned long now = millis();

  // Handle LOP call from Floor 1 (UP button only)
  bool lop1Up = (bitRead(inputData2, LOP_CALL_UP_1_BIT) == 0);
  if (lop1Up && (now - lastButtonPressTime[LOP_CALL_UP_1_BIT]) > DEBOUNCE_DELAY_MS) {
    if (1 != currentFloor) {
      callUp[0] = true;
      Serial.println("Panggilan LOP Naik dari Lantai 1 ditambahkan ke antrean.");
      lastButtonPressTime[LOP_CALL_UP_1_BIT] = now;
      lastActivityTime = now;
    }
  }

  // Handle LOP calls from Floor 2 (UP and DOWN buttons)
  bool lop2Up = (bitRead(inputData2, LOP_CALL_UP_2_BIT) == 0);
  if (lop2Up && (now - lastButtonPressTime[LOP_CALL_UP_2_BIT]) > DEBOUNCE_DELAY_MS) {
    if (2 != currentFloor) {
      callUp[1] = true;
      Serial.println("Panggilan LOP Naik dari Lantai 2 ditambahkan ke antrean.");
      lastButtonPressTime[LOP_CALL_UP_2_BIT] = now;
      lastActivityTime = now;
    }
  }

  bool lop2Down = (bitRead(inputData2, LOP_CALL_DOWN_2_BIT) == 0);
  if (lop2Down && (now - lastButtonPressTime[LOP_CALL_DOWN_2_BIT]) > DEBOUNCE_DELAY_MS) {
    if (2 != currentFloor) {
      callDown[1] = true;
      Serial.println("Panggilan LOP Turun dari Lantai 2 ditambahkan ke antrean.");
      lastButtonPressTime[LOP_CALL_DOWN_2_BIT] = now;
      lastActivityTime = now;
    }
  }

  // Handle LOP call from Floor 3 (DOWN button only)
  bool lop3Down = (bitRead(inputData2, LOP_CALL_DOWN_3_BIT) == 0);
  if (lop3Down && (now - lastButtonPressTime[LOP_CALL_DOWN_3_BIT]) > DEBOUNCE_DELAY_MS) {
    if (3 != currentFloor) {
      callDown[2] = true;
      Serial.println("Panggilan LOP Turun dari Lantai 3 ditambahkan ke antrean.");
      lastButtonPressTime[LOP_CALL_DOWN_3_BIT] = now;
      lastActivityTime = now;
    }
  }
}

void handleLimitSwitches() {
    unsigned long now = millis();
    for (int i = 0; i < totalFloors; i++) {
        bool currentLimitStatus = (bitRead(inputData2, i) == 0);
        if (currentLimitStatus != lastLimitStatus[i] && (now - lastLimitDebounceTime[i]) > LIMIT_DEBOUNCE_DELAY_MS) {
            if (currentLimitStatus) {
                currentFloor = i + 1;
                copCallActive[i] = false;
                lastActivityTime = now;
                Serial.print("Limit switch terpicu di lantai ");
                Serial.println(currentFloor);
                Serial1.print(currentFloor);
                Serial1.print("\n");
                Serial.print("Mengirim notifikasi ke dfmniplayer: lantai ");
                Serial.println(currentFloor);
                printTimestamp();
                int nextFloor = findNextFloor();
                if (nextFloor == currentFloor) {
                    stopMotor();
                    callUp[i] = false;
                    callDown[i] = false;
                    currentState = DOOR_OPENING;
                    stateTimer = now;
                    Serial.println("Mencapai lantai tujuan. Pintu akan terbuka.");
                    printTimestamp();
                }
            }
            lastLimitStatus[i] = currentLimitStatus;
            lastLimitDebounceTime[i] = now;
        }
    }
}

void handleMotorTimeout();

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

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

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

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

    if (currentState == DOOR_OPENING || currentState == DOOR_CLOSING) {
        bitSet(outputData1, DOOR_LED);
    } else {
        bitClear(outputData1, DOOR_LED);
    }

    if (cabinLightOn) {
        bitSet(outputData1, CABIN_LIGHT_LED);
    } else {
        bitClear(outputData1, CABIN_LIGHT_LED);
    }
    if (isEmergency || currentState == CALIBRATION_FAILED || currentState == EMERGENCY_STOP) {
        bitSet(outputData1, ERROR_LED);
        bitSet(outputData2, BUZZER_BIT);
        bitClear(outputData1, SYSTEM_OK_LED);
    } else {
        bitSet(outputData1, SYSTEM_OK_LED);
        bitClear(outputData1, ERROR_LED);
        bitClear(outputData2, BUZZER_BIT);
    }
    writeAllSROutputs();
}