#include "mesh_publish.h"
#include "mesh_validate.h"
#include <Arduino.h>
#include <SPI.h>

static const char* TAG = "MeshPublish";

// RadioLib SX1262 instance — pins set in mesh_init
static SX1262* s_radio = nullptr;
static bool s_radio_ready = false;

// ---------------------------------------------------------------------------
// Initialize SX1262 radio
// ---------------------------------------------------------------------------
bool mesh_init(SPIClass& spi) {
    // Create SX1262 module: NSS, DIO1, NRST, BUSY
    static Module mod(LORA_SS, LORA_DIO1, LORA_RST, LORA_BUSY, spi);
    static SX1262 radio(&mod);
    s_radio = &radio;

    Serial.printf("[%s] Initializing SX1262...\n", TAG);

    int state = s_radio->begin(
        LORA_FREQ_MHZ,
        LORA_BANDWIDTH_KHZ,
        LORA_SPREAD_FACTOR,
        LORA_CODING_RATE,
        LORA_SYNC_WORD,
        LORA_TX_POWER_DBM,
        LORA_PREAMBLE_LEN
    );

    if (state != RADIOLIB_ERR_NONE) {
        Serial.printf("[%s] SX1262 begin() failed, code %d\n", TAG, state);
        return false;
    }

    // Use DIO2 as RF switch control (standard for Heltec V4)
    state = s_radio->setDio2AsRfSwitch(true);
    if (state != RADIOLIB_ERR_NONE) {
        Serial.printf("[%s] setDio2AsRfSwitch failed, code %d\n", TAG, state);
    }

    // Set CRC on
    state = s_radio->setCRC(2);  // 2-byte CRC
    if (state != RADIOLIB_ERR_NONE) {
        Serial.printf("[%s] setCRC failed, code %d\n", TAG, state);
    }

    s_radio_ready = true;
    Serial.printf("[%s] SX1262 ready. Freq=%.1f MHz, SF=%d, BW=%.0f kHz, TX=%d dBm\n",
                  TAG, LORA_FREQ_MHZ, LORA_SPREAD_FACTOR, LORA_BANDWIDTH_KHZ, LORA_TX_POWER_DBM);
    return true;
}

// ---------------------------------------------------------------------------
// Serialize DetectionEvent to wire format
// ---------------------------------------------------------------------------
size_t mesh_serialize_event(const DetectionEvent& evt, uint8_t* buf, size_t buf_len) {
    if (buf_len < DETECTION_PACKET_SIZE) return 0;

    size_t pos = 0;

    buf[pos++] = evt.msg_type;

    // sensor_id (4 bytes, big-endian)
    buf[pos++] = (evt.sensor_id >> 24) & 0xFF;
    buf[pos++] = (evt.sensor_id >> 16) & 0xFF;
    buf[pos++] = (evt.sensor_id >> 8)  & 0xFF;
    buf[pos++] = (evt.sensor_id)       & 0xFF;

    // timestamp_utc (4 bytes, big-endian)
    buf[pos++] = (evt.timestamp_utc >> 24) & 0xFF;
    buf[pos++] = (evt.timestamp_utc >> 16) & 0xFF;
    buf[pos++] = (evt.timestamp_utc >> 8)  & 0xFF;
    buf[pos++] = (evt.timestamp_utc)       & 0xFF;

    buf[pos++] = evt.class_label;
    buf[pos++] = evt.confidence;

    // rssi (2 bytes, signed, big-endian)
    buf[pos++] = (evt.rssi >> 8) & 0xFF;
    buf[pos++] = (evt.rssi)      & 0xFF;

    // noise_floor (2 bytes, signed, big-endian)
    buf[pos++] = (evt.noise_floor >> 8) & 0xFF;
    buf[pos++] = (evt.noise_floor)      & 0xFF;

    // signature_hash (8 bytes)
    memcpy(&buf[pos], evt.signature_hash, 8);
    pos += 8;

    // lat (4 bytes, big-endian)
    buf[pos++] = (evt.lat_fixed >> 24) & 0xFF;
    buf[pos++] = (evt.lat_fixed >> 16) & 0xFF;
    buf[pos++] = (evt.lat_fixed >> 8)  & 0xFF;
    buf[pos++] = (evt.lat_fixed)       & 0xFF;

    // lon (4 bytes, big-endian)
    buf[pos++] = (evt.lon_fixed >> 24) & 0xFF;
    buf[pos++] = (evt.lon_fixed >> 16) & 0xFF;
    buf[pos++] = (evt.lon_fixed >> 8)  & 0xFF;
    buf[pos++] = (evt.lon_fixed)       & 0xFF;

    buf[pos++] = evt.battery_pct;
    buf[pos++] = evt.flags;

    return pos;
}

// ---------------------------------------------------------------------------
// Deserialize wire format to DetectionEvent
// ---------------------------------------------------------------------------
bool mesh_deserialize_event(const uint8_t* buf, size_t len, DetectionEvent& evt) {
    if (len < DETECTION_PACKET_SIZE) return false;

    size_t pos = 0;

    evt.msg_type = buf[pos++];

    evt.sensor_id = ((uint32_t)buf[pos] << 24) | ((uint32_t)buf[pos+1] << 16) |
                    ((uint32_t)buf[pos+2] << 8) | buf[pos+3];
    pos += 4;

    evt.timestamp_utc = ((uint32_t)buf[pos] << 24) | ((uint32_t)buf[pos+1] << 16) |
                        ((uint32_t)buf[pos+2] << 8) | buf[pos+3];
    pos += 4;

    evt.class_label = buf[pos++];
    evt.confidence  = buf[pos++];

    evt.rssi = (int16_t)(((uint16_t)buf[pos] << 8) | buf[pos+1]);
    pos += 2;

    evt.noise_floor = (int16_t)(((uint16_t)buf[pos] << 8) | buf[pos+1]);
    pos += 2;

    memcpy(evt.signature_hash, &buf[pos], 8);
    pos += 8;

    evt.lat_fixed = ((int32_t)buf[pos] << 24) | ((int32_t)buf[pos+1] << 16) |
                    ((int32_t)buf[pos+2] << 8) | buf[pos+3];
    pos += 4;

    evt.lon_fixed = ((int32_t)buf[pos] << 24) | ((int32_t)buf[pos+1] << 16) |
                    ((int32_t)buf[pos+2] << 8) | buf[pos+3];
    pos += 4;

    evt.battery_pct = buf[pos++];
    evt.flags       = buf[pos++];

    // Structural plausibility gate — see mesh_validate.h. Without this, any
    // foreign LoRa frame (Meshtastic traffic shares our PHY closely enough to
    // pass CRC) whose first byte matched a PDL message type was parsed into a
    // DetectionEvent and relayed to C2 as a real contact. Rejecting here
    // covers BOTH relay paths at once, since main.cpp's MSG_TYPE_DETECTION and
    // MSG_TYPE_HEALTH cases each gate their C2 post on this function's return.
    if (!mesh_event_is_plausible(evt)) return false;

    return true;
}

// ---------------------------------------------------------------------------
// Publish a detection event
// ---------------------------------------------------------------------------
bool mesh_publish_detection(const DetectionEvent& evt) {
    if (!s_radio_ready) {
        Serial.printf("[%s] Radio not initialized\n", TAG);
        return false;
    }

    uint8_t packet[DETECTION_PACKET_SIZE];
    size_t len = mesh_serialize_event(evt, packet, sizeof(packet));
    if (len == 0) return false;

    int state = s_radio->transmit(packet, len);
    if (state == RADIOLIB_ERR_NONE) {
        Serial.printf("[%s] TX detection: class=%d conf=%d rssi=%d (%u bytes)\n",
                      TAG, evt.class_label, evt.confidence, evt.rssi, len);
        return true;
    } else {
        Serial.printf("[%s] TX failed, code %d\n", TAG, state);
        return false;
    }
}

// ---------------------------------------------------------------------------
// Publish raw packet
// ---------------------------------------------------------------------------
bool mesh_publish_raw(const uint8_t* data, size_t len) {
    if (!s_radio_ready) return false;

    int state = s_radio->transmit(const_cast<uint8_t*>(data), len);
    if (state == RADIOLIB_ERR_NONE) {
        return true;
    }
    Serial.printf("[%s] Raw TX failed, code %d\n", TAG, state);
    return false;
}

// ---------------------------------------------------------------------------
// Non-blocking receive
// ---------------------------------------------------------------------------
int mesh_receive(uint8_t* buffer, size_t max_len, float& rssi, float& snr) {
    if (!s_radio_ready) return 0;

    size_t len = 0;
    int state = s_radio->readData(buffer, max_len);

    if (state == RADIOLIB_ERR_NONE) {
        len = s_radio->getPacketLength();
        rssi = s_radio->getRSSI();
        snr  = s_radio->getSNR();
        return (int)len;
    }

    return 0;
}

// ---------------------------------------------------------------------------
// Start continuous receive mode
// ---------------------------------------------------------------------------
void mesh_start_receive() {
    if (!s_radio_ready) return;
    int state = s_radio->startReceive();
    if (state != RADIOLIB_ERR_NONE) {
        Serial.printf("[%s] startReceive() failed, code %d\n", TAG, state);
    }
}

SX1262& mesh_get_radio() {
    return *s_radio;
}
