#include "hardware_rfid.hpp"
#include <Arduino.h>
#include <SPI.h>

// #define DEBUG_RFID
#ifdef DEBUG_RFID
#define LOG(msg) Serial.println(msg)
#else
#define LOG(msg)
#endif

HardwareRfid::HardwareRfid(uint8_t ssPin, uint8_t rstPin)
    : _mfrc(ssPin, rstPin) {}

void HardwareRfid::begin() {
    LOG("RFID begin called");
    SPI.begin(); // Initialize SPI bus
    _mfrc.PCD_Init();
    
    // Verify MFRC522 hardware
    uint8_t version = _mfrc.PCD_ReadRegister(_mfrc.VersionReg);
    if (version == 0x00 || version == 0xFF) {
        LOG("ERROR: Failed to detect MFRC522 hardware. Check wiring or power.");
    } else {
        LOG("MFRC522 detected, version: 0x" + String(version, HEX));
    }
    
    // Optional: Set antenna gain for better range
    _mfrc.PCD_SetAntennaGain(_mfrc.RxGain_max);
    LOG("RFID initialized, antenna gain set to max");
}

void HardwareRfid::end() {
    LOG("RFID end called");
    SPI.end(); // Stop SPI bus
}

void HardwareRfid::update() {
    static unsigned long lastLog = 0;
    unsigned long now = millis();
    if (now - lastLog >= 1000) {
        LOG("RFID update called");
        lastLog = now;
    }

    // Check if a new card is present
    if (!_mfrc.PICC_IsNewCardPresent()) {
        if (now - lastLog >= 1000) {
            LOG("No new card present");
        }
        return;
    }

    // Try to read the card's serial number
    if (!_mfrc.PICC_ReadCardSerial()) {
        LOG("Failed to read card serial");
        return;
    }

    // Get the UID
    uint32_t cardId = 0;
    for (byte i = 0; i < _mfrc.uid.size; i++) {
        cardId = (cardId << 8) | _mfrc.uid.uidByte[i];
    }
    LOG("Card detected, UID: 0x" + String(cardId, HEX));

    // Create the message
    hardware_SensorToControlMessage msg = hardware_SensorToControlMessage_init_zero;
    msg.sensor_id = 0; // Assuming single sensor
    msg.which_payload = hardware_SensorToControlMessage_rfid_reading_tag;
    msg.payload.rfid_reading.card_id = cardId;

    // Call the callback if set
    if (_callback) {
        LOG("Callback is set, calling it with card ID: 0x" + String(cardId, HEX));
        _callback(msg);
    } else {
        LOG("Callback not set");
    }

    // Halt the card and stop crypto
    _mfrc.PICC_HaltA();
    _mfrc.PCD_StopCrypto1();
}

void HardwareRfid::setCallback(RfidCallback cb) {
    _callback = cb;
    LOG("RFID callback set");
}