// ============================================================================
// meshtastic_emit.cpp
//
// Pragmatic minimum viable Meshtastic emit/parse for an embedded sensor.
// We do NOT pull in the full Meshtastic firmware — instead we hand-roll
// the wire-frame envelope and embed a tiny nanopb-style varint encoder for
// the inner Data { portnum, payload } message. This keeps RAM/flash budget
// tiny while still being interoperable with stock Meshtastic clients.
// ============================================================================

#include "meshtastic_emit.h"
#include "radio_scheduler.h"
#include "config.h"

#include <cstring>
#include <cstdio>

#ifdef ARDUINO
#include <Arduino.h>
#include <nvs_flash.h>
#include <nvs.h>
#include "mesh_publish.h"
#include "enrollment.h"
#endif

#ifndef ARDUINO
#include <cstdint>
static uint32_t s_virt_ms_mt = 0;
static uint32_t MILLIS_MT() { return s_virt_ms_mt; }
// Stub enrollment for native tests
static uint32_t enrollment_get_sensor_id_native_stub() { return 0xCB0D5033; }
#define ENROLL_ID() enrollment_get_sensor_id_native_stub()
#else
#define MILLIS_MT() millis()
#define ENROLL_ID() enrollment_get_sensor_id()
#endif

static const char* TAG_MT = "Meshtastic";

static bool s_mt_enabled = false;
static char s_node_name[13] = "PDL-Sensor";
static uint32_t s_last_nodeinfo_ms = 0;
static uint32_t s_packet_id_seq = 1;
static MeshtasticStats s_mt_stats;
// hide collision with other modules' s_stats when test_interop pulls them
// all into the same translation unit

// ---------------------------------------------------------------------------
// Tiny varint encoder (proto3 style)
// ---------------------------------------------------------------------------
static size_t pb_varint(uint8_t* buf, size_t buf_len, size_t pos, uint64_t v) {
    while (v >= 0x80) {
        if (pos >= buf_len) return 0;
        buf[pos++] = (uint8_t)(v & 0x7F) | 0x80;
        v >>= 7;
    }
    if (pos >= buf_len) return 0;
    buf[pos++] = (uint8_t)v;
    return pos;
}

static size_t pb_field_tag(uint8_t* buf, size_t buf_len, size_t pos,
                           uint32_t field_no, uint32_t wire_type) {
    return pb_varint(buf, buf_len, pos, ((uint64_t)field_no << 3) | wire_type);
}

// ---------------------------------------------------------------------------
// Build inner Data submessage
//   message Data {
//     uint32 portnum  = 1;  // varint
//     bytes  payload  = 2;  // length-delimited
//   }
// ---------------------------------------------------------------------------
static size_t build_data(uint8_t portnum, const uint8_t* payload, size_t plen,
                         uint8_t* out, size_t out_len) {
    size_t pos = 0;
    pos = pb_field_tag(out, out_len, pos, 1, 0);          // portnum tag
    if (!pos) return 0;
    pos = pb_varint(out, out_len, pos, portnum);
    if (!pos) return 0;

    pos = pb_field_tag(out, out_len, pos, 2, 2);          // payload tag (LD)
    if (!pos) return 0;
    pos = pb_varint(out, out_len, pos, plen);
    if (!pos) return 0;
    if (pos + plen > out_len) return 0;
    memcpy(&out[pos], payload, plen);
    pos += plen;
    return pos;
}

// ---------------------------------------------------------------------------
// Build envelope around an inner-data buffer
//   [0..3]  dest LE
//   [4..7]  src  LE
//   [8..11] pid  LE
//   [12]    flags
//   [13]    channel_hash
//   [14..15] reserved
//   [16..]  inner data
// ---------------------------------------------------------------------------
static size_t build_envelope(uint32_t src, uint32_t dst, uint32_t pid,
                             const uint8_t* inner, size_t inner_len,
                             uint8_t* out, size_t out_len) {
    if (out_len < 16 + inner_len) return 0;
    out[0]  = (uint8_t)(dst);
    out[1]  = (uint8_t)(dst >> 8);
    out[2]  = (uint8_t)(dst >> 16);
    out[3]  = (uint8_t)(dst >> 24);
    out[4]  = (uint8_t)(src);
    out[5]  = (uint8_t)(src >> 8);
    out[6]  = (uint8_t)(src >> 16);
    out[7]  = (uint8_t)(src >> 24);
    out[8]  = (uint8_t)(pid);
    out[9]  = (uint8_t)(pid >> 8);
    out[10] = (uint8_t)(pid >> 16);
    out[11] = (uint8_t)(pid >> 24);
    out[12] = 0x00;                          // flags: no want_ack
    out[13] = MESHTASTIC_CHANNEL_HASH;
    out[14] = 0x00;
    out[15] = 0x00;
    memcpy(&out[16], inner, inner_len);
    return 16 + inner_len;
}

// ---------------------------------------------------------------------------
// Public — radio params hook (registered weak in radio_scheduler.cpp)
// ---------------------------------------------------------------------------
int radio_mesh_set_params() {
#ifdef ARDUINO
    SX1262& r = mesh_get_radio();
    int st;
    st = r.setFrequency(MESHTASTIC_FREQ_MHZ);     if (st) return st;
    st = r.setBandwidth(MESHTASTIC_BW_KHZ);       if (st) return st;
    st = r.setSpreadingFactor(MESHTASTIC_SF);     if (st) return st;
    st = r.setCodingRate(MESHTASTIC_CR);          if (st) return st;
    st = r.setSyncWord(MESHTASTIC_SYNC);          if (st) return st;
    st = r.setPreambleLength(MESHTASTIC_PREAMBLE);if (st) return st;
    return 0;
#else
    return 0;
#endif
}

// ---------------------------------------------------------------------------
// Public lifecycle
// ---------------------------------------------------------------------------
bool meshtastic_init() {
    memset(&s_mt_stats, 0, sizeof(s_mt_stats));
    s_mt_enabled = false;

#ifdef ARDUINO
    nvs_handle_t h;
    if (nvs_open(MESHTASTIC_NVS_NS, NVS_READONLY, &h) == ESP_OK) {
        uint8_t en = 0;
        if (nvs_get_u8(h, MESHTASTIC_NVS_ENABLED, &en) == ESP_OK && en == 1) {
            s_mt_enabled = true;
        }
        size_t name_len = sizeof(s_node_name);
        nvs_get_str(h, MESHTASTIC_NVS_NODE_NAME, s_node_name, &name_len);
        nvs_close(h);
    }
    Serial.printf("[%s] init enabled=%d name=%s\n",
                  TAG_MT, s_mt_enabled ? 1 : 0, s_node_name);
#else
    s_mt_enabled = true;  // native tests always treat as enabled
#endif
    return s_mt_enabled;
}

bool meshtastic_is_mt_enabled() { return s_mt_enabled; }

void meshtastic_get_stats(MeshtasticStats* out) {
    if (out) *out = s_mt_stats;
}

// ---------------------------------------------------------------------------
// Emit NodeInfo (port 4): payload is just our short_name encoded as ASCII.
// Real Meshtastic NodeInfo is a richer User submessage but a vendor-specific
// node-name string is a forwards-compatible degraded form that stock clients
// will display as "unknown node + payload bytes".
// ---------------------------------------------------------------------------
bool meshtastic_emit_nodeinfo() {
    if (!s_mt_enabled) return false;
    uint32_t now = MILLIS_MT();
    if (now - s_last_nodeinfo_ms < MESHTASTIC_NODEINFO_PERIOD_MS) return false;
    if (!radio_sched_can_tx(RADIO_MODE_MESHTASTIC)) return false;

    uint8_t inner[64];
    size_t inner_len = build_data(MT_PORTNUM_NODEINFO,
                                   (const uint8_t*)s_node_name,
                                   strlen(s_node_name),
                                   inner, sizeof(inner));
    if (inner_len == 0) { s_mt_stats.tx_failures++; return false; }

    uint8_t pkt[MESHTASTIC_MAX_PACKET];
    size_t pkt_len = build_envelope(ENROLL_ID(), 0xFFFFFFFFu,
                                    s_packet_id_seq++, inner, inner_len,
                                    pkt, sizeof(pkt));
    if (pkt_len == 0) { s_mt_stats.tx_failures++; return false; }

#ifdef ARDUINO
    bool ok = mesh_publish_raw(pkt, pkt_len);
    if (ok) { s_mt_stats.tx_nodeinfo++; s_last_nodeinfo_ms = now; return true; }
    s_mt_stats.tx_failures++;
    return false;
#else
    s_mt_stats.tx_nodeinfo++;
    s_last_nodeinfo_ms = now;
    return true;
#endif
}

// ---------------------------------------------------------------------------
// Emit text message (port 1)
// ---------------------------------------------------------------------------
bool meshtastic_emit_text(const char* msg) {
    if (!s_mt_enabled || !msg) return false;
    if (!radio_sched_can_tx(RADIO_MODE_MESHTASTIC)) {
        // Request a 200ms lease to push this packet
        if (!radio_sched_request_mode(RADIO_MODE_MESHTASTIC, 200)) {
            return false;
        }
    }

    uint8_t inner[MESHTASTIC_MAX_PACKET];
    size_t inner_len = build_data(MT_PORTNUM_TEXT_MSG,
                                   (const uint8_t*)msg,
                                   strlen(msg),
                                   inner, sizeof(inner));
    if (inner_len == 0) { s_mt_stats.tx_failures++; return false; }

    uint8_t pkt[MESHTASTIC_MAX_PACKET];
    size_t pkt_len = build_envelope(ENROLL_ID(), 0xFFFFFFFFu,
                                    s_packet_id_seq++, inner, inner_len,
                                    pkt, sizeof(pkt));
    if (pkt_len == 0) { s_mt_stats.tx_failures++; return false; }

#ifdef ARDUINO
    bool ok = mesh_publish_raw(pkt, pkt_len);
    if (ok) { s_mt_stats.tx_text++; return true; }
    s_mt_stats.tx_failures++;
    return false;
#else
    s_mt_stats.tx_text++;
    return true;
#endif
}

// ---------------------------------------------------------------------------
// Inbound parse — returns parsed bytes, also extracts portnum + src for
// the interop router.
// ---------------------------------------------------------------------------
static size_t pb_read_varint(const uint8_t* buf, size_t len, size_t pos,
                             uint64_t* out) {
    uint64_t v = 0;
    int shift = 0;
    while (pos < len && shift < 64) {
        uint8_t b = buf[pos++];
        v |= (uint64_t)(b & 0x7F) << shift;
        if ((b & 0x80) == 0) { *out = v; return pos; }
        shift += 7;
    }
    return 0;
}

static int parse_inner_data(const uint8_t* in, size_t len,
                            uint8_t* out_portnum,
                            const uint8_t** out_payload, size_t* out_plen) {
    size_t pos = 0;
    *out_portnum = 0;
    *out_payload = nullptr;
    *out_plen = 0;
    while (pos < len) {
        uint64_t tag = 0;
        pos = pb_read_varint(in, len, pos, &tag);
        if (pos == 0) return -1;
        uint32_t field_no = (uint32_t)(tag >> 3);
        uint32_t wire = (uint32_t)(tag & 0x7);
        if (field_no == 1 && wire == 0) {
            uint64_t v = 0;
            pos = pb_read_varint(in, len, pos, &v);
            if (pos == 0) return -1;
            *out_portnum = (uint8_t)v;
        } else if (field_no == 2 && wire == 2) {
            uint64_t plen = 0;
            pos = pb_read_varint(in, len, pos, &plen);
            if (pos == 0) return -1;
            if (pos + plen > len) return -1;
            *out_payload = &in[pos];
            *out_plen = (size_t)plen;
            pos += plen;
        } else {
            // skip unknown fields conservatively
            if (wire == 0) {
                uint64_t v = 0;
                pos = pb_read_varint(in, len, pos, &v);
                if (pos == 0) return -1;
            } else if (wire == 2) {
                uint64_t plen = 0;
                pos = pb_read_varint(in, len, pos, &plen);
                if (pos == 0) return -1;
                if (pos + plen > len) return -1;
                pos += plen;
            } else {
                return -1; // 32/64-bit not supported
            }
        }
    }
    return 0;
}

int meshtastic_handle_packet(const uint8_t* buf, size_t len) {
    if (!s_mt_enabled) return -1;
    if (len < 16) { s_mt_stats.rx_parse_failures++; return 0; }

    uint32_t dst = (uint32_t)buf[0] | ((uint32_t)buf[1] << 8) |
                   ((uint32_t)buf[2] << 16) | ((uint32_t)buf[3] << 24);
    uint32_t src = (uint32_t)buf[4] | ((uint32_t)buf[5] << 8) |
                   ((uint32_t)buf[6] << 16) | ((uint32_t)buf[7] << 24);
    (void)dst;

    uint8_t portnum = 0;
    const uint8_t* payload = nullptr;
    size_t plen = 0;
    int rc = parse_inner_data(&buf[16], len - 16, &portnum, &payload, &plen);
    if (rc != 0) { s_mt_stats.rx_parse_failures++; return 0; }

    s_mt_stats.rx_packets++;

#ifdef ARDUINO
    // Wrap as MSG_TYPE_INTEROP_RX and emit to PDL mesh outbox.
    // Format:
    //   [0]      = MSG_TYPE_INTEROP_RX (0x21)
    //   [1]      = source-protocol = 1 (Meshtastic)
    //   [2..5]   = src node id (LE)
    //   [6]      = portnum
    //   [7]      = payload length (truncated to 200 max)
    //   [8..]    = payload bytes
    uint8_t out_pkt[256];
    out_pkt[0] = MSG_TYPE_INTEROP_RX;
    out_pkt[1] = 1;
    out_pkt[2] = (uint8_t)src;
    out_pkt[3] = (uint8_t)(src >> 8);
    out_pkt[4] = (uint8_t)(src >> 16);
    out_pkt[5] = (uint8_t)(src >> 24);
    out_pkt[6] = portnum;
    size_t copy_len = plen > 200 ? 200 : plen;
    out_pkt[7] = (uint8_t)copy_len;
    memcpy(&out_pkt[8], payload, copy_len);
    // Note: this fires while we're still in MESHTASTIC mode; the PDL TX
    // will be deferred until the scheduler returns to PDL mode (handled
    // by interop_router queueing, see interop_router.cpp).
    extern void interop_router_enqueue_pdl(const uint8_t* p, size_t l);
    interop_router_enqueue_pdl(out_pkt, 8 + copy_len);
#endif
    return (int)len;
}

// ---------------------------------------------------------------------------
// Native test helpers
// ---------------------------------------------------------------------------
#ifdef UNIT_TEST
extern "C" size_t meshtastic_test_build_nodeinfo(uint8_t* out, size_t out_len) {
    const char* name = "PDL-Test";
    uint8_t inner[64];
    size_t il = build_data(MT_PORTNUM_NODEINFO,
                           (const uint8_t*)name, strlen(name),
                           inner, sizeof(inner));
    if (il == 0) return 0;
    return build_envelope(0xCB0D5033u, 0xFFFFFFFFu, 1, inner, il, out, out_len);
}
extern "C" size_t meshtastic_test_build_text(const char* msg, uint8_t* out, size_t out_len) {
    uint8_t inner[MESHTASTIC_MAX_PACKET];
    size_t il = build_data(MT_PORTNUM_TEXT_MSG,
                           (const uint8_t*)msg, strlen(msg),
                           inner, sizeof(inner));
    if (il == 0) return 0;
    return build_envelope(0xCB0D5033u, 0xFFFFFFFFu, 2, inner, il, out, out_len);
}
extern "C" int meshtastic_test_parse(const uint8_t* in, size_t len,
                                     uint32_t* out_src, uint8_t* out_portnum) {
    if (len < 16) return -1;
    *out_src = (uint32_t)in[4] | ((uint32_t)in[5] << 8) |
               ((uint32_t)in[6] << 16) | ((uint32_t)in[7] << 24);
    uint8_t portnum = 0;
    const uint8_t* payload = nullptr;
    size_t plen = 0;
    int rc = parse_inner_data(&in[16], len - 16, &portnum, &payload, &plen);
    if (rc != 0) return rc;
    *out_portnum = portnum;
    return 0;
}
#endif
