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

// Makro untuk konfigurasi bit
#ifndef LOP_UP_BASE_BIT
  #define LOP_UP_BASE_BIT   16   // bit offset untuk tombol LOP UP
#endif
#ifndef LOP_DOWN_BASE_BIT
  #define LOP_DOWN_BASE_BIT 24   // bit offset untuk tombol LOP DOWN
#endif
#define MAX_SR_BITS 32

// 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}; // Untuk tombol COP
unsigned long lastLOPUpTime[16] = {0};       // Untuk tombol LOP naik
unsigned long lastLOPDownTime[16] = {0};     // Untuk tombol LOP turun
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 lastActivityTime = 0;
unsigned long lastBlink = 0;
bool blinkState = true;
unsigned long previousMillis = 0;
int dotCount = 0;
unsigned long lastCabinLightActivityTime = 0;

// ===================================== setup() =====================================
void setup() {
    Serial.begin(115200);
    Serial.println("Orch_ 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();

    initSerial();
    updateFloorLeds();
    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");
    }
    lastActivityTime = millis();
}

// ===================================== loop() =====================================
void loop() {
    // 1. Baca semua input di awal loop untuk data terbaru
    readAllSRInputs();

    // 2. Tangani darurat sebagai prioritas tertinggi
    if (handleEmergencyState() || currentState == CALIBRATION_FAILED) {
        displayStatus();
        updateFloorLeds();
        return; // Keluar dari loop jika darurat
    }

    // 3. Proses input tombol dan serial
    handleSerialCommands();
    processCallButtons();
    processDoorAndLightButtons();
    handleLimitSwitches();
    handleMotorTimeout();

    // 4. Tangani perubahan mode
    handleModeChange();

    // 5. Jalankan logika utama berdasarkan mode operasional
    switch (currentMode) {
        case MODE_NORMAL:
            handleNormalMode();
            break;
        case MODE_TEST:
            handleTestMode();
            break;
        case MODE_INSPECTION:
            handleInspectionMode();
            break;
        default:
            currentMode = MODE_NORMAL;
            break;
    }

    // 6. Perbarui output visual dan fisik
    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");
                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;
            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.printf("Panggilan UART ke Lantai %d (NAIK)\n", floor);
                } else if (floor < currentFloor) {
                    callDown[floor - 1] = true;
                    Serial.printf("Panggilan UART ke Lantai %d (TURUN)\n", floor);
                } else {
                    if (currentState == IDLE || currentState == DOOR_OPEN) {
                        currentState = DOOR_OPENING;
                        stateTimer = millis();
                        Serial.println("Panggilan ke lantai saat ini. Memicu pembukaan pintu.");
                    }
                }
            }
        }
    }
    
    static ElevatorState lastState = currentState;
    if (lastState != currentState) {
        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: {
            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() {
    processCallButtons();
    processDoorAndLightButtons();
    handleLimitSwitches();
    handleNormalMode();
}

void handleInspectionMode() {
    bool insUpButton = (bitRead(inputData1, INS_UP_BUTTON_BIT) == 0);
    bool insDownButton = (bitRead(inputData1, INS_DOWN_BUTTON_BIT) == 0);

    if (insUpButton) {
        if (currentState != MOVING_UP) {
            Serial.println("INSPECTION: Moving UP.");
        }
        currentState = MOVING_UP;
        controlMotor(currentState);
    } else if (insDownButton) {
        if (currentState != MOVING_DOWN) {
            Serial.println("INSPECTION: Moving DOWN.");
        }
        currentState = MOVING_DOWN;
        controlMotor(currentState);
    } else {
        if (currentState != IDLE) {
            Serial.println("INSPECTION: STOP.");
        }
        stopMotor();
        currentState = IDLE;
    }
    handleLimitSwitches();
}

// ===================================== Fungsi Pembantu =====================================
// void handleFinalLimits() { }
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.printf("Total Floors: %d\n", totalFloors);
}

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

    if (openButton || closeButton) {
        lastActivityTime = now;
        cabinLightOn = true;
    }

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

    bool currentCabinLightButtonState = (bitRead(inputData1, CABIN_LIGHT_BUTTON_BIT) == 0);
    if (lastCabinLightButtonState == HIGH && currentCabinLightButtonState == LOW && (now - lastCabinLightTime) > DEBOUNCE_DELAY_MS) {
        cabinLightOn = !cabinLightOn;
        lastCabinLightTime = now;
        if (cabinLightOn) lastCabinLightActivityTime = now;
        Serial.printf("Lampu Kabin: %s\n", cabinLightOn ? "ON" : "OFF");
    }
    lastCabinLightButtonState = currentCabinLightButtonState;

    for (int i = 0; i < totalFloors; i++) {
        bool copCall = (bitRead(inputData1, i) == 0);
        if (copCall) {
            lastCabinLightActivityTime = now;
            cabinLightOn = true;
        }
    }
    if (cabinLightOn && (now - lastCabinLightActivityTime) > CABIN_LIGHT_TIMEOUT_MS) {
        cabinLightOn = false;
        Serial.println("Lampu kabin dimatikan karena timeout.");
    }
}

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.printf("Limit switch terpicu di lantai %d\n", currentFloor);
                Serial1.printf("%d\n", currentFloor);
                Serial.printf("Mengirim notifikasi ke dfmniplayer: lantai %d\n", currentFloor);
                
                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.");
                }
            }
            lastLimitStatus[i] = currentLimitStatus;
            lastLimitDebounceTime[i] = now;
        }
    }
}

bool handleEmergencyState() {
    static bool lastEmergencyPinState = HIGH;
    bool currentEmergencyPinState = digitalRead(EMERGENCY_PIN) == LOW;

    if (currentEmergencyPinState && !lastEmergencyPinState) {
        if (!isEmergency) {
            isEmergency = true;
            currentState = EMERGENCY_STOP;
            stopMotor();
            Serial.println("Tombol darurat ditekan!");
        }
    } else if (!currentEmergencyPinState && lastEmergencyPinState) {
        if (isEmergency) {
            isEmergency = false;
            currentState = EMERGENCY_RESETTING;
            Serial.println("Reset darurat.");
        }
    }
    lastEmergencyPinState = currentEmergencyPinState;
    return isEmergency;
}

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

void handleEmergency() {
    handleEmergencyState();
}

// ===================================== Fungsi Penjadwalan & Panggilan =====================================
readAllSRInputs();

void processCallButtons() {
    uint32_t srInputs = (uint32_t)inputData1 | ((uint32_t)inputData2 << 8);

    handleCOPCalls(srInputs);
    handleLOPCalls(srInputs);
}

void handleCOPCalls(uint32_t srInputs) {
    unsigned long now = millis();
    for (int i = 0; i < totalFloors; i++) {
        bool isButtonPressed = (bitRead(srInputs, COP_BASE_BIT + i) == 0);
        if (isButtonPressed && (now - lastButtonPressTime[i]) > DEBOUNCE_DELAY_MS) {
            lastActivityTime = now;
            int floorNumber = i + 1;

            if (floorNumber == currentFloor) {
                if (currentState == IDLE || currentState == DOOR_OPEN) {
                    currentState = DOOR_OPENING;
                    stateTimer = now;
                    Serial.printf("COP: Lantai %d (current). Membuka pintu.\n", floorNumber);
                }
            } else {
                if (floorNumber > currentFloor) {
                    callUp[i] = true;
                    copCallActive[i] = true;
                    Serial.printf("COP: Panggilan naik ke lantai %d.\n", floorNumber);
                } else {
                    callDown[i] = true;
                    copCallActive[i] = true;
                    Serial.printf("COP: Panggilan turun ke lantai %d.\n", floorNumber);
                }
            }
            lastButtonPressTime[i] = now;
        }
    }
}

void handleLOPCalls(uint32_t srInputs) {
    unsigned long now = millis();
    for (int i = 0; i < totalFloors; i++) {
        if (i < totalFloors - 1) { // LOP UP
            bool isUpButtonPressed = (bitRead(srInputs, LOP_UP_BASE_BIT + i) == 0);
            if (isUpButtonPressed && (now - lastLOPUpTime[i]) > DEBOUNCE_DELAY_MS) {
                callUp[i] = true;
                lastLOPUpTime[i] = now;
                lastActivityTime = now;
                Serial.printf("LOP: Panggilan NAIK dari lantai %d.\n", i + 1);
            }
        }
        if (i > 0) { // LOP DOWN
            bool isDownButtonPressed = (bitRead(srInputs, LOP_DOWN_BASE_BIT + (i - 1)) == 0);
            if (isDownButtonPressed && (now - lastLOPDownTime[i]) > DEBOUNCE_DELAY_MS) {
                callDown[i] = true;
                lastLOPDownTime[i] = now;
                lastActivityTime = now;
                Serial.printf("LOP: Panggilan TURUN dari lantai %d.\n", i + 1);
            }
        }
    }
}

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