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

// 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;
// debounce etc
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;
// display timing
unsigned long lastBlink = 0;
bool blinkState = true;
unsigned long previousMillis = 0;
int dotCount = 0;

// Prototypes of functions still used by main file
void handleModeChange();
void handleNormalMode();
void handleTestMode();
void handleInspectionMode();
void processCallButtons();
void processDoorAndLightButtons();
void handleLimitSwitches();
void handleEmergency();

void setup() {
  // 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);

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

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    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");
  }
}

void loop() {
  readAllSRInputs();
  handleEmergency();
  handleModeChange();

  if (bitRead(inputData2, LIMIT_FINAL_UP_BIT) == 0 || bitRead(inputData2, LIMIT_FINAL_DOWN_BIT) == 0) {
    if (!isEmergency) {
      Serial.println("FINAL LIMIT SWITCH TERPICU! Lift dihentikan karena over-travel.");
      isEmergency = true;
      currentState = EMERGENCY_STOP;
      stopMotor();
      bitSet(outputData2, EMERGENCY_LED_BIT);
    }
  }

  if (isEmergency || currentState == CALIBRATION_FAILED) {
    displayStatus();
    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 (dipindah dari kode awal) ==================
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.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;
            stateTimer = millis();
            Serial.println("Panggilan ke lantai saat ini. Memicu pembukaan pintu.");
          }
        }
      }
    }
  }

  processDoorAndLightButtons();
  handleLimitSwitches();
  handleMotorTimeout();

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

// =================================== Fungsi Tambahan ===================================
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(); // prototype already in motor.h but keep to avoid warnings

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 && (currentState == DOOR_CLOSING || currentState == IDLE || currentState == DOOR_OPEN)) {
    currentState = DOOR_OPENING;
    stateTimer = now;
    return;
  }
  if (closeButton && currentState == DOOR_OPEN) {
    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;
    Serial.print("Lampu Kabin: ");
    Serial.println(cabinLightOn ? "ON" : "OFF");
  }
  lastCabinLightButtonState = currentCabinLightButtonState;
}

void processCallButtons() {
  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) {
      if (i + 1 == currentFloor) {
        if (currentState == IDLE || currentState == DOOR_OPEN) {
          Serial.println("Tombol lantai saat ini ditekan. Memicu pembukaan pintu.");
          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.");
        } 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.");
        }
      }
      lastButtonPressTime[i] = now;
    }
  }
  bool callUpButtonState = (bitRead(inputData1, LOP_CALL_UP_BIT) == 0);
  bool callDownButtonState = (bitRead(inputData1, LOP_CALL_DOWN_BIT) == 0);
  if (callUpButtonState && (now - lastCallUpTime) > 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.");
    }
    lastCallUpTime = now;
  }
  if (callDownButtonState && (now - lastCallDownTime) > 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.");
    }
    lastCallDownTime = 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;
        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);
        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;
    }
  }
}

void handleMotorTimeout(); // prototype in motor.h

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

// updateFloorLeds & SR outputs
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_UP_LED_BIT);
  } else {
    bitClear(outputData1, LOP_CALL_UP_LED_BIT);
  }
  bool anyCallDown = false;
  for (int i = 0; i < totalFloors; i++) {
    if (callDown[i]) {
      anyCallDown = true;
      break;
    }
  }
  if (anyCallDown) {
    bitSet(outputData1, LOP_CALL_DOWN_LED_BIT);
  } else {
    bitClear(outputData1, LOP_CALL_DOWN_LED_BIT);
  }
  if (bitRead(inputData1, DOOR_OPEN_BUTTON_BIT) == 0) {
    bitSet(outputData1, DOOR_OPEN_INDICATOR_BIT);
  } else {
    bitClear(outputData1, DOOR_OPEN_INDICATOR_BIT);
  }
  if (bitRead(inputData1, DOOR_CLOSE_BUTTON_BIT) == 0) {
    bitSet(outputData1, DOOR_CLOSE_INDICATOR_BIT);
  } else {
    bitClear(outputData1, DOOR_CLOSE_INDICATOR_BIT);
  }
  if (cabinLightOn) {
    bitSet(outputData1, CABIN_LIGHT_BIT);
  } else {
    bitClear(outputData1, CABIN_LIGHT_BIT);
  }
  if (isEmergency || currentState == CALIBRATION_FAILED) {
    bitSet(outputData2, EMERGENCY_LED_BIT);
    bitSet(outputData2, BUZZER_BIT);
  } else {
    bitClear(outputData2, EMERGENCY_LED_BIT);
    bitClear(outputData2, BUZZER_BIT);
  }
  writeAllSROutputs();
}