/* LOP_Firmware.ino
 * File Sketsa Utama Arduino (Ino) - Integrasi Penuh
 * Menggunakan standar 18-byte biner (CRC16-CCITT) untuk kompatibilitas penuh 
 * dengan MCC, FPS, dan CTB.
 */

#include <Arduino.h>
#include <MD_Parola.h>
#include <MD_MAX72xx.h>
#include <SPI.h>
// File header yang berisi DEFINISI PROTOKOL dan DEKLARASI fungsi logger
#include "LiftLogger.h" 

// =================================================================================
// 1. PIN MAPPING & DEVICE CONFIG (KONFIGURASI LOKAL)
// =================================================================================
#ifndef FLOOR_NUMBER
  #define FLOOR_NUMBER 1 // LOP ini berada di Lantai 1. Sesuaikan dengan LOP yang sebenarnya.
#endif

// Pinout ESP32
constexpr int BTN_UP   = 33;
constexpr int BTN_DOWN = 32;
constexpr int LED_UP   = 27;
constexpr int LED_DOWN = 26;
constexpr int LOCAL_LED_PIN = 2; // Untuk indikasi TX/RX di LOP
constexpr int RS485_DE_PIN = 4; // Pin Data Enable RS485

// Matrix Display Configuration
constexpr int DATA_PIN1   = 23;
constexpr int CLK_PIN     = 18;
constexpr int CS_PIN1     = 5;
constexpr int MAX_DEVICES1 = 2; // Lantai Display

constexpr int CS_PIN2     = 4; // CS Pin untuk Display Arah (PERHATIAN: Pin ini sama dengan RS485_DE_PIN.
                               // Di sini diasumsikan pin 4 hanya dipakai untuk RS485_DE_PIN, dan CS_PIN2 akan diubah
                               // agar tidak konflik. **Pin 4 digunakan untuk RS485 DE.** Mari ganti CS_PIN2 ke pin lain.
                               // MENGUBAH CS_PIN2 ke 13 (Pin yang umum digunakan)
constexpr int CS_PIN_ARAH = 13; 
constexpr int MAX_DEVICES2 = 1; // Arah Display

// =================================================================================
// 2. PROTOCOL & DEFINITIONS (HANYA CONSTANTS LOKAL)
// =================================================================================
// Konstanta sudah diimpor dari LiftLogger.h

// =================================================================================
// 3. INTERNAL DEFINITIONS & LOPController CLASS
// =================================================================================

enum CommMode { DEBUG, HWS };
enum LiftDir { IDLE, UP, DOWN };

/* Timing & Pengulangan */
const unsigned long DEBOUNCE_MS = 200;
const unsigned long CALL_RATE_LIMIT_MS = 400; 
const unsigned long RX_FSM_TIMEOUT_MS = 10;

/* --- LOPController Structure --- */
struct LOPController {
private:
  CommMode currentMode = DEBUG;
  LiftDir arahLift = IDLE;
  int lantai = 1;
  bool newArah = true;
  int lastLantaiDisplayed = -1;

  unsigned long lastBtnMillis[2] = {0, 0};
  unsigned long lastSentMillis[2] = {0, 0};
  bool lastBtnState[2] = {HIGH, HIGH};
  bool ledState[2] = {false, false};
  
  uint32_t crc_error_count = 0;
  uint8_t tx_seq = 0;
  const NodeID_t MY_NODE_ID = (NodeID_t)(NODE_ID_LOP_BASE + (FLOOR_NUMBER - 1));

  // Buffer RX (Fixed Length FSM)
  enum RxState { WAIT_HEADER1, WAIT_HEADER2, READ_PAYLOAD };
  static const size_t RX_BUF_SZ = LIFT_PACKET_SIZE;
  uint8_t rxBuf[RX_BUF_SZ];
  size_t rxLen = 0;
  RxState rx_state = WAIT_HEADER1;
  unsigned long rx_fsm_timeout_start = 0;
  
  // Instance Display
  // Menggunakan CS_PIN_ARAH (13) untuk mencegah konflik dengan RS485_DE_PIN (4)
  MD_Parola P_Lantai;
  MD_MAX72XX P_Arah;

  // Fungsi Protokol
  uint16_t crc16_ccitt(const uint8_t *buf, size_t len);
  void sendPacket(NodeID_t dest_id, Command_t cmd, uint16_t status_mask, uint8_t floor_num);
  
  // Fungsi Logika RX
  void rxPollProd();
  bool receivePacket(LiftPacket_t& rx_pkt);
  void processInboundPacket(const LiftPacket_t& rx_pkt);
  void process_inbound_status(const LiftPacket_t& rx_pkt);

  // Fungsi UI/Debug
  void setLedValidation(bool upOn, bool downOn);
  void displayArrowFrame(const byte frame[8]);
  void selfCheck();
  void handleDebugTextRx(const String &msg);

public:
  // Konstruktor
  LOPController()
    : P_Lantai(MD_MAX72XX::PAROLA_HW, DATA_PIN1, CLK_PIN, CS_PIN1, MAX_DEVICES1),
      P_Arah(MD_MAX72XX::PAROLA_HW, DATA_PIN1, CLK_PIN, CS_PIN_ARAH, MAX_DEVICES2) {}

  void begin();
  void handleSerialCommand(const String &cmd);
  CommMode getCurrentMode() const { return currentMode; }
  void loop(); 
  void btnScan();
  void updateDisplay();
};

// =================================================================================
// 4. METHOD IMPLEMENTATIONS
// =================================================================================

// --- CRC16-CCITT ---
uint16_t LOPController::crc16_ccitt(const uint8_t *buf, size_t len) {
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= (uint16_t)buf[i] << 8;
        for (int j = 0; j < 8; j++) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

// --- Pengiriman Paket Biner (18-byte) ---
void LOPController::sendPacket(NodeID_t dest_id, Command_t cmd, uint16_t status_mask, uint8_t floor_num) {
    LiftPacket_t tx_pkt;

    tx_pkt.header1 = 0xAA;
    tx_pkt.header2 = 0x55;
    tx_pkt.magic = 0xDE;
    tx_pkt.protocol_version = (1 << 4) | 2; // V1.2
    tx_pkt.source_node_id = MY_NODE_ID; 
    tx_pkt.dest_node_id = dest_id;
    tx_pkt.sequence_id = ++tx_seq;
    tx_pkt.command = cmd;
    
    // Data Aplikasi
    tx_pkt.status_mask = status_mask;       
    tx_pkt.floor_number = floor_num;        
    tx_pkt.crc_error_count = crc_error_count & 0xFF;
    tx_pkt.floor_call_mask = 0; // LOP tidak mengisi field ini
    
    // Hitung CRC
    tx_pkt.crc16 = crc16_ccitt(((uint8_t*)&tx_pkt) + 2, CRC_PAYLOAD_LEN);  

    uint8_t *raw_data = (uint8_t*)&tx_pkt;

    // --- TIER 1 LOGGING: PARSED LOG ---
    logParsedPacket(tx_pkt, "TX", true); 
    
    // Kirim data melalui Serial2
    digitalWrite(RS485_DE_PIN, HIGH); 
    
    Serial2.write(raw_data, LIFT_PACKET_SIZE); 
    Serial2.flush();

    // FIX KOMPATIBILITAS: Tunggu transmisi selesai. 
    // Pada 115200 bps, 18 byte membutuhkan ~1.56ms. delay(2) memberikan margin aman.
    delay(2); 
    
    digitalWrite(RS485_DE_PIN, LOW); // Kembali ke mode RX
}

// --- RX Polling & Parsing (Fixed Length FSM) ---
bool LOPController::receivePacket(LiftPacket_t& rx_pkt) {
    uint32_t current_time = millis();

    // Timeout FSM
    if (rx_state == READ_PAYLOAD && (current_time - rx_fsm_timeout_start >= RX_FSM_TIMEOUT_MS)) {
        Serial.println("[LOP RX] FSM Timeout. Resetting parser.");
        rx_state = WAIT_HEADER1;
        rxLen = 0;
    }
    
    while (Serial2.available()) {
        char ch = (char)Serial2.read();
        
        switch (rx_state) {
            case WAIT_HEADER1:
                if (ch == 0xAA) { rxBuf[0] = ch; rx_state = WAIT_HEADER2; }
                break;
            case WAIT_HEADER2:
                if (ch == 0x55) { 
                    rxBuf[1] = ch; 
                    rxLen = 2; 
                    rx_state = READ_PAYLOAD; 
                    rx_fsm_timeout_start = current_time;
                } else { 
                    rx_state = WAIT_HEADER1; 
                } 
                break;

            case READ_PAYLOAD:
                rxBuf[rxLen++] = ch;
                rx_fsm_timeout_start = current_time;

                if (rxLen >= LIFT_PACKET_SIZE) { 
                    memcpy(&rx_pkt, rxBuf, LIFT_PACKET_SIZE);
                    rx_state = WAIT_HEADER1; 
                    rxLen = 0; // Reset length for next packet
                    
                    // Filter: Hanya proses yang ditujukan ke LOP ini atau Broadcast
                    if (rx_pkt.dest_node_id != MY_NODE_ID && rx_pkt.dest_node_id != NODE_ID_BROADCAST) continue;

                    // Validasi CRC
                    uint16_t calc_crc = crc16_ccitt(((uint8_t*)&rx_pkt) + 2, CRC_PAYLOAD_LEN); 
                    if (calc_crc != rx_pkt.crc16) {
                        crc_error_count++;
                        Serial.printf("[LOP RX] CRC FAIL! Cmd: %d, Count: %lu. Logging raw data...\n", rx_pkt.command, crc_error_count);
                        
                        // LOGGING: RAW BINARY UNTUK MENDIAGNOSIS KESALAHAN CRC
                        logRawPacket(rxBuf, LIFT_PACKET_SIZE); 
                        
                        continue; 
                    }
                    
                    // LOGGING: PARSED LOG UNTUK PAKET BERHASIL
                    logParsedPacket(rx_pkt, "RX", true); 

                    return true;
                }
                break;
        }
    }
    return false;
}

void LOPController::rxPollProd() {
    LiftPacket_t rx_pkt;
    while (receivePacket(rx_pkt)) {
        processInboundPacket(rx_pkt);
    }
}

// --- Logika Aplikasi ---
void LOPController::processInboundPacket(const LiftPacket_t& rx_pkt) {
    if (rx_pkt.command == CMD_STATUS_REPORT) {
        process_inbound_status(rx_pkt);
    } else if (rx_pkt.command == CMD_ACK_CALL) {
        Serial.printf("[LOP RX] ACK diterima dari 0x%02X untuk Seq ID: %d\n", rx_pkt.source_node_id, rx_pkt.sequence_id);
    } else {
        Serial.printf("[LOP RX] Paket diterima, CMD tidak diproses: %d\n", rx_pkt.command);
    }
}

void LOPController::process_inbound_status(const LiftPacket_t& rx_pkt) {
    // 1. Update Lantai
    int current_f = rx_pkt.floor_number;
    if (current_f != lantai) {
        lantai = current_f;
        lastLantaiDisplayed = -1; // Paksa display update
    }
    
    // 2. Update Arah
    LiftDir nd = IDLE;
    if (rx_pkt.status_mask & STATUS_MASK_UP_DIR) nd = UP;
    else if (rx_pkt.status_mask & STATUS_MASK_DOWN_DIR) nd = DOWN;

    if (nd != arahLift) newArah = true;
    arahLift = nd;

    // 3. Update LED Panggilan (Validation)
    // Floor Call Mask di paket STATUS_REPORT dari MCC akan berisi bitmask untuk 
    // semua panggilan yang masih aktif. LOP harus mengecek bitnya sendiri.
    uint32_t floor_bit = 1UL << (FLOOR_NUMBER - 1);
    
    // LOP menganggap tombolnya menyala jika MCC mengindikasikan 
    // LOP CALL UP (bit 7) ATAU LOP CALL DN (bit 8) masih aktif.
    bool led_up_on = (rx_pkt.floor_call_mask & (1UL << (FLOOR_NUMBER - 1))) && (rx_pkt.status_mask & STATUS_MASK_LOP_CALL_UP);
    bool led_dn_on = (rx_pkt.floor_call_mask & (1UL << (FLOOR_NUMBER - 1))) && (rx_pkt.status_mask & STATUS_MASK_LOP_CALL_DN);

    // Di sini disederhanakan: kita asumsikan MCC hanya mengirim 1 bit per lantai, 
    // dan LOP memantau bit panggilannya sendiri (misal: bit 0 untuk Lantai 1)
    // Jika MCC mengimplementasikan bitmask yang lebih detail, logika di bawah perlu disesuaikan.
    bool is_call_active = (rx_pkt.floor_call_mask & floor_bit) != 0;
    
    // Asumsi: LED LOP menyala jika bit-nya aktif di mask MCC.
    // LOP tidak membedakan LED up/down dari status MCC (karena LOP biasanya punya 2 tombol di 1 titik).
    // Jika LOP adalah LOP 1-tombol per lantai, ini cukup. Jika LOP 2-tombol, kita asumsikan 
    // LED_UP/LED_DOWN adalah LED tunggal/terpisah untuk indikasi panggilan.
    setLedValidation(is_call_active, is_call_active); 
}

void LOPController::setLedValidation(bool upOn, bool downOn) {
    ledState[0] = upOn;
    ledState[1] = downOn;
    digitalWrite(LED_UP, upOn ? HIGH : LOW);
    digitalWrite(LED_DOWN, downOn ? HIGH : LOW);
}

// --- Kontrol Tombol ---
void LOPController::btnScan() {
  int pins[2] = { BTN_UP, BTN_DOWN };
  uint16_t dir_masks[2] = { STATUS_MASK_UP_DIR, STATUS_MASK_DOWN_DIR };

  for (int i=0;i<2;i++) {
    // Membaca keadaan tombol (LOW jika ditekan)
    bool s = digitalRead(pins[i]);
    
    // Logika debounce: tombol baru ditekan, keadaan sebelumnya HIGH, dan sudah lewat waktu debounce
    if (s == LOW && lastBtnState[i] == HIGH && (millis() - lastBtnMillis[i]) > DEBOUNCE_MS) {
      lastBtnMillis[i] = millis();
      lastBtnState[i] = s;
      
      // Rate Limit: Memastikan tidak mengirim panggilan terlalu cepat
      if (millis() - lastSentMillis[i] > CALL_RATE_LIMIT_MS) {
          // LOP mengirim CMD_CALL dengan status_mask yang menunjukkan arah
          sendPacket(NODE_ID_MCC, CMD_CALL, dir_masks[i], FLOOR_NUMBER);
          lastSentMillis[i] = millis();
          Serial.printf("[EVENT] BTN%d pressed -> CALL sent\n", i + 1);
          digitalWrite(LOCAL_LED_PIN, HIGH); delay(40); digitalWrite(LOCAL_LED_PIN, LOW); // Blink TX
      } else {
          Serial.printf("[WARN] BTN%d pressed -> Rate Limited\n", i + 1);
      }
    } else if (s == HIGH) {
      // Tombol dilepas
      lastBtnState[i] = s;
    }
  }
}

// --- Display Logic ---
const byte ARROW_UP_FADE_SEQUENCE[8][8] = {
  {0,0,0,0,0,0,0,0}, {0,0,0,0,0,0,0b00011000,0b00011000},
  {0,0,0,0,0,0b00111100,0b01111110,0b00011000}, {0,0,0,0,0b00011000,0b00111100,0b01111110,0b11111111},
  {0,0,0b00011000,0b00111100,0b01111110,0b11111111,0b00011000,0b00011000},
  {0b00011000,0b00111100,0b01111110,0b11111111,0b00011000,0b00011000,0,0},
  {0b01111110,0b11111111,0b00011000,0b00011000,0,0,0,0}, {0b11111111,0b00011000,0b00011000,0,0,0,0,0}
};
const byte ARROW_DOWN_FADE_SEQUENCE[8][8] = {
  {0,0,0,0,0,0,0,0}, {0b00011000,0b00011000,0,0,0,0,0,0},
  {0b00011000,0b01111110,0b00111100,0,0,0,0,0}, {0b11111111,0b01111110,0b00111100,0b00011000,0,0,0,0},
  {0b00011000,0b00011000,0b11111111,0b01111110,0b00111100,0b00011000,0,0},
  {0,0,0b00011000,0b00011000,0b11111111,0b01111110,0b00111100,0b00011000},
  {0,0,0,0,0b00011000,0b00011000,0b11111111,0b01111110}, {0,0,0,0,0,0,0b00011000,0b00011000}
};
const int FADE_FRAMES = 8;

void LOPController::displayArrowFrame(const byte frame[8]) {
  P_Arah.clear();
  for (int row = 0; row < 8; row++) P_Arah.setRow(0, row, frame[row]);
}

void LOPController::updateDisplay() {
  // 1. Update Lantai
  if (lantai != lastLantaiDisplayed) {
    String lantaiText = String(lantai);
    P_Lantai.displayText(lantaiText.c_str(), PA_CENTER, 0, 0, PA_NO_EFFECT, PA_NO_EFFECT);
    P_Lantai.displayAnimate();
    Serial.println("[DISPLAY] Lantai: " + lantaiText);
    lastLantaiDisplayed = lantai;
  }
  P_Lantai.displayAnimate();

  // 2. Animasi Arah
  static unsigned long lastFrameTime = 0;
  static const unsigned long frameDelay = 100;
  static int currentFrame = 0;
  static bool wasCleared = false; 

  if (newArah) {
    Serial.println("[DISPLAY] Arah berubah: " + String(arahLift == UP ? "UP" : arahLift == DOWN ? "DOWN" : "IDLE"));
    currentFrame = 0;
    lastFrameTime = millis();
    newArah = false;
    wasCleared = false;
  }

  if (arahLift == UP) {
    if (millis() - lastFrameTime >= frameDelay) {
      displayArrowFrame(ARROW_UP_FADE_SEQUENCE[currentFrame]);
      currentFrame = (currentFrame + 1) % FADE_FRAMES;
      lastFrameTime = millis();
    }
  } else if (arahLift == DOWN) {
    if (millis() - lastFrameTime >= frameDelay) {
      displayArrowFrame(ARROW_DOWN_FADE_SEQUENCE[currentFrame]);
      currentFrame = (currentFrame + 1) % FADE_FRAMES;
      lastFrameTime = millis();
    }
  } else {
    // IDLE
    if (!wasCleared) {
      P_Arah.clear();
      wasCleared = true;
    }
  }
}

// --- Debug & Mode Control ---
void LOPController::handleDebugTextRx(const String &msg) {
    if (msg.length() == 0) return;
    
    // Simulasikan paket CMD_STATUS_REPORT masuk (untuk testing UI)
    if (msg.startsWith("STATUS:")) {
        int floor = 0, dir = 0;
        uint32_t call_mask = 0;
        if (sscanf(msg.c_str(), "STATUS:F%d|D%d|M%lu", &floor, &dir, &call_mask) != 3) {
            Serial.println("[DBG ERR] Format Status tidak valid. Gunakan: STATUS:F<lantai>|D<arah>|M<mask>");
            return;
        }
        
        LiftPacket_t mock_pkt = {}; // Inisialisasi struct
        mock_pkt.command = CMD_STATUS_REPORT;
        mock_pkt.floor_number = (uint8_t)floor;
        mock_pkt.floor_call_mask = call_mask;
        mock_pkt.source_node_id = NODE_ID_MCC;
        mock_pkt.dest_node_id = MY_NODE_ID;
        mock_pkt.header1 = 0xAA;
        mock_pkt.header2 = 0x55;
        mock_pkt.magic = 0xDE;
        mock_pkt.protocol_version = (1 << 4) | 2; 

        if (dir == 1) mock_pkt.status_mask |= STATUS_MASK_UP_DIR;
        else if (dir == 2) mock_pkt.status_mask |= STATUS_MASK_DOWN_DIR;

        Serial.printf("[DBG] Simulating Status: F%d D%d M%lu\n", floor, dir, call_mask);
        process_inbound_status(mock_pkt);

    } else if (msg.startsWith("CALL:")) {
        // Simulasikan pengiriman CMD_CALL dari LOP
        int dir = 0;
        if (sscanf(msg.c_str(), "CALL:%d", &dir) != 1) {
            Serial.println("[DBG ERR] Format Call tidak valid. Gunakan: CALL:1 (UP) atau CALL:2 (DOWN)");
            return;
        }
        if (dir == 1) { // UP
            sendPacket(NODE_ID_MCC, CMD_CALL, STATUS_MASK_UP_DIR, FLOOR_NUMBER);
        } else if (dir == 2) { // DOWN
            sendPacket(NODE_ID_MCC, CMD_CALL, STATUS_MASK_DOWN_DIR, FLOOR_NUMBER);
        }
        
    } else {
        Serial.print("[RX TXT] Unknown command: "); Serial.println(msg);
    }
}

void LOPController::handleSerialCommand(const String &rawCmd) {
  String cmd = rawCmd;
  cmd.trim();

  if (cmd.equalsIgnoreCase("mode hardware")) {
    currentMode = HWS;
    Serial.println("[MODE] Switched to HWS (Serial2 for MCC)");
  } else if (cmd.equalsIgnoreCase("mode debug")) {
    currentMode = DEBUG;
    Serial.println("[MODE] Switched to DEBUG (Serial for MCC)");
  } else {
    if (currentMode == DEBUG) {
      // Perintah tidak dikenal di mode DEBUG dianggap sebagai simulasi MCC
      handleDebugTextRx(cmd);
    } else {
      Serial.print("[CMD] Unknown: "); Serial.println(cmd);
    }
  }
}

void LOPController::selfCheck() {
  Serial.println("=== LOP Self-Check ===");
  pinMode(LOCAL_LED_PIN, OUTPUT);
  for (int i=0;i<2;i++) {
    digitalWrite(LED_UP, HIGH); 
    digitalWrite(LED_DOWN, HIGH); 
    digitalWrite(LOCAL_LED_PIN, HIGH); delay(120);
    digitalWrite(LED_UP, LOW); 
    digitalWrite(LED_DOWN, LOW); 
    digitalWrite(LOCAL_LED_PIN, LOW); delay(120);
  }
  P_Lantai.displayText("SC", PA_CENTER, 0, 0, PA_SCROLL_LEFT, PA_SCROLL_LEFT);
  P_Lantai.displayAnimate();
  delay(800);
  P_Lantai.displayClear();
  Serial.printf("Self-Check OK. LOP ID: 0x%X\n", MY_NODE_ID);
}

// =================================================================================
// 5. ARDUINO LIFECYCLE (setup() dan loop() global)
// =================================================================================

// Membuat instance dari Controller (seperti objek "global")
LOPController controller;

void setup() {
  // Panggil fungsi setup dari Controller
  controller.begin();
}

void LOPController::begin() {
  // Pin I/O
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(LED_UP, OUTPUT);
  pinMode(LED_DOWN, OUTPUT);
  pinMode(LOCAL_LED_PIN, OUTPUT); // Pin 2 untuk indikator
  pinMode(RS485_DE_PIN, OUTPUT); // RS485_DE_PIN = 4
  digitalWrite(RS485_DE_PIN, LOW); // Start in RX mode
  
  // Serial
  Serial.begin(RS485_BAUD_RATE);
  delay(50);
  Serial.println("LOP Final Firmware (Binary Protocol) starting...");
  // Serial2: RX=16, TX=17
  Serial2.begin(RS485_BAUD_RATE, SERIAL_8N1, 16, 17); 
  Serial.println("Serial2 ready for Hardware comms");

  // Display init
  P_Lantai.begin();
  P_Lantai.setIntensity(5);
  P_Lantai.displayClear();
  
  P_Arah.begin();
  P_Arah.control(MD_MAX72XX::INTENSITY, 5);
  P_Arah.clear();

  // Initial states
  setLedValidation(false, false);
  
  selfCheck();
  Serial.println("Ready. Default mode=DEBUG. Use 'mode hardware' to switch.");
}

void loop() {
  controller.loop();
}

void LOPController::loop() {
  // 1) Penanganan Perintah Serial (Pengalihan Mode & Debug RX)
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    handleSerialCommand(cmd);
  }

  // 2) Poll RX dari PROD serial saat di mode PROD
  if (currentMode == HWS) {
    rxPollProd();
  }

  // 3) Pindai tombol
  btnScan();

  // 4) Update Display (Lantai & Arah) - Panggilan LED diupdate via rxPollProd
  updateDisplay();

  // small yield
  delay(1);
}