// ============================================================================
// T1 UAV Relay — STANAG 4586 LoRa → serial MAVLink bridge
// ============================================================================

#include "uav_relay.h"

#ifdef PDL_ENABLE_UAV_RELAY
#include <cstring>
#include "mesh_publish.h"
#include "config.h"
#include "uav_pairing.h"

#ifdef ARDUINO
#include <Arduino.h>
#include <HardwareSerial.h>
#else
// Host-side stubs so this module (previously buildable ONLY on-device, and
// only in the separate PDL_ENABLE_UAV_RELAY build variant -- not compiled
// into Spencer01's active firmware today) can be exercised by a native
// Unity test. Same #ifdef ARDUINO pattern used throughout this sweep.
#include <cstdint>
#include <cstddef>
#define SERIAL_8N1 0
class HardwareSerial {
public:
    explicit HardwareSerial(int) {}
    void begin(unsigned long, int, int, int) {}
    // Test-observable via a weak hook so tests can capture what would have
    // gone out to the drone modem UART without a real HardwareSerial.
    size_t write(const uint8_t* data, size_t len);
};
extern "C" __attribute__((weak))
void uav_relay_test_capture_uart_write(const uint8_t* data, size_t len);
inline size_t HardwareSerial::write(const uint8_t* data, size_t len) {
    if (uav_relay_test_capture_uart_write) uav_relay_test_capture_uart_write(data, len);
    return len;
}
#endif // ARDUINO
#endif // PDL_ENABLE_UAV_RELAY

// Module-local state (only used when PDL_ENABLE_UAV_RELAY is defined).
#ifdef PDL_ENABLE_UAV_RELAY
namespace {
    bool           s_initialized = false;
    uint8_t        s_hmac_key[32] = {0};
    size_t         s_hmac_key_len = 0;
    uint32_t       s_own_sensor_id_hint = 0;    // set via uav_relay_set_own_sensor_id()
    HardwareSerial s_drone_uart(1);             // UART1; TX = GPIO 17, RX = GPIO 18

    // Minimal MAVLink v2 header constants — full libmavlink would be too
    // heavy for a T1 relay. We emit the minimum viable frames for:
    //   MAV_CMD_NAV_WAYPOINT          (16)
    //   MAV_CMD_DO_SET_MODE           (176)
    //   MAV_CMD_NAV_RETURN_TO_LAUNCH  (20)
    //   MAV_CMD_MISSION_START         (300)
    constexpr uint8_t MAVLINK_STX_V2 = 0xFD;
    constexpr uint8_t MAV_COMPID_UCS = 190;     // onboard UAV controller ID
    constexpr uint8_t MAV_SYSID_GCS  = 255;

    uint32_t crc16_mavlink(const uint8_t* buf, size_t len) {
        // Standard CRC-16-CCITT init 0xFFFF; minimal, unused checksum for stub
        uint16_t crc = 0xFFFF;
        for (size_t i = 0; i < len; i++) {
            uint8_t b = buf[i] ^ (crc & 0xFF);
            b ^= b << 4;
            crc = (crc >> 8) ^ ((uint16_t)b << 8) ^ ((uint16_t)b << 3) ^ ((uint16_t)b >> 4);
        }
        return crc;
    }

    // Pack body region into native-endian fields for MAVLink synth
    struct DecodedBody {
        int32_t lat_e7;     // only meaningful for LLA / waypoint messages
        int32_t lon_e7;
        int16_t alt_m;
        uint16_t heading_x10;
        uint8_t  raw_first_byte;
    };

    DecodedBody decode_body(const uint8_t* body) {
        DecodedBody d = {};
        d.lat_e7 = (int32_t)(((uint32_t)body[0] << 24) | ((uint32_t)body[1] << 16) |
                             ((uint32_t)body[2] << 8)  |  (uint32_t)body[3]);
        d.lon_e7 = (int32_t)(((uint32_t)body[4] << 24) | ((uint32_t)body[5] << 16) |
                             ((uint32_t)body[6] << 8)  |  (uint32_t)body[7]);
        d.alt_m  = (int16_t)(((uint16_t)body[8] << 8) | (uint16_t)body[9]);
        d.heading_x10 = (uint16_t)(((uint16_t)body[10] << 8) | (uint16_t)body[11]);
        d.raw_first_byte = body[0];
        return d;
    }

    uint8_t s_seq_counter = 0;

    // ------------------------------------------------------------------
    // HMAC-SHA256 (truncated to 4 bytes) — ported from drone-companion's
    // src/stanag_decoder.cpp, the reference implementation for this exact
    // wire protocol (drone-companion/SPEC.md documents the shared format:
    // HMAC-SHA256(root, bytes[0..28])[0..4]). Self-contained (no mbedtls
    // dependency) so it compiles identically on-device and under the
    // native UNIT_TEST build. This REPLACES the previous no-op stub that
    // unconditionally passed every frame through regardless of its HMAC.
    // ------------------------------------------------------------------
    struct Sha256Ctx {
        uint32_t state[8];
        uint64_t bitlen;
        uint8_t  buffer[64];
        uint32_t buf_len;
    };

    constexpr uint32_t K[64] = {
        0x428a2f98,0x71374491,0xb5c0fbcf,0xe9b5dba5,0x3956c25b,0x59f111f1,0x923f82a4,0xab1c5ed5,
        0xd807aa98,0x12835b01,0x243185be,0x550c7dc3,0x72be5d74,0x80deb1fe,0x9bdc06a7,0xc19bf174,
        0xe49b69c1,0xefbe4786,0x0fc19dc6,0x240ca1cc,0x2de92c6f,0x4a7484aa,0x5cb0a9dc,0x76f988da,
        0x983e5152,0xa831c66d,0xb00327c8,0xbf597fc7,0xc6e00bf3,0xd5a79147,0x06ca6351,0x14292967,
        0x27b70a85,0x2e1b2138,0x4d2c6dfc,0x53380d13,0x650a7354,0x766a0abb,0x81c2c92e,0x92722c85,
        0xa2bfe8a1,0xa81a664b,0xc24b8b70,0xc76c51a3,0xd192e819,0xd6990624,0xf40e3585,0x106aa070,
        0x19a4c116,0x1e376c08,0x2748774c,0x34b0bcb5,0x391c0cb3,0x4ed8aa4a,0x5b9cca4f,0x682e6ff3,
        0x748f82ee,0x78a5636f,0x84c87814,0x8cc70208,0x90befffa,0xa4506ceb,0xbef9a3f7,0xc67178f2
    };

    inline uint32_t rotr(uint32_t x, uint32_t n) { return (x >> n) | (x << (32 - n)); }

    void sha256_init(Sha256Ctx& c) {
        c.state[0]=0x6a09e667; c.state[1]=0xbb67ae85; c.state[2]=0x3c6ef372; c.state[3]=0xa54ff53a;
        c.state[4]=0x510e527f; c.state[5]=0x9b05688c; c.state[6]=0x1f83d9ab; c.state[7]=0x5be0cd19;
        c.bitlen = 0; c.buf_len = 0;
    }

    void sha256_transform(Sha256Ctx& c, const uint8_t* d) {
        uint32_t w[64];
        for (int i = 0; i < 16; i++) {
            w[i] = ((uint32_t)d[i*4] << 24) | ((uint32_t)d[i*4+1] << 16) |
                   ((uint32_t)d[i*4+2] << 8) | (uint32_t)d[i*4+3];
        }
        for (int i = 16; i < 64; i++) {
            uint32_t s0 = rotr(w[i-15],7) ^ rotr(w[i-15],18) ^ (w[i-15] >> 3);
            uint32_t s1 = rotr(w[i-2],17) ^ rotr(w[i-2],19)  ^ (w[i-2]  >> 10);
            w[i] = w[i-16] + s0 + w[i-7] + s1;
        }
        uint32_t a=c.state[0],b=c.state[1],cc=c.state[2],d2=c.state[3];
        uint32_t e=c.state[4],f=c.state[5],g=c.state[6],h=c.state[7];
        for (int i = 0; i < 64; i++) {
            uint32_t S1 = rotr(e,6) ^ rotr(e,11) ^ rotr(e,25);
            uint32_t ch = (e & f) ^ ((~e) & g);
            uint32_t t1 = h + S1 + ch + K[i] + w[i];
            uint32_t S0 = rotr(a,2) ^ rotr(a,13) ^ rotr(a,22);
            uint32_t mj = (a & b) ^ (a & cc) ^ (b & cc);
            uint32_t t2 = S0 + mj;
            h=g; g=f; f=e; e=d2+t1; d2=cc; cc=b; b=a; a=t1+t2;
        }
        c.state[0]+=a; c.state[1]+=b; c.state[2]+=cc; c.state[3]+=d2;
        c.state[4]+=e; c.state[5]+=f; c.state[6]+=g;  c.state[7]+=h;
    }

    void sha256_update(Sha256Ctx& c, const uint8_t* d, size_t len) {
        for (size_t i = 0; i < len; i++) {
            c.buffer[c.buf_len++] = d[i];
            if (c.buf_len == 64) { sha256_transform(c, c.buffer); c.bitlen += 512; c.buf_len = 0; }
        }
    }

    void sha256_final(Sha256Ctx& c, uint8_t out[32]) {
        uint32_t i = c.buf_len;
        c.buffer[i++] = 0x80;
        if (i > 56) { while (i < 64) c.buffer[i++] = 0; sha256_transform(c, c.buffer); i = 0; }
        while (i < 56) c.buffer[i++] = 0;
        c.bitlen += (uint64_t)c.buf_len * 8;
        for (int j = 7; j >= 0; j--) c.buffer[i++] = (uint8_t)(c.bitlen >> (j * 8));
        sha256_transform(c, c.buffer);
        for (int j = 0; j < 8; j++) {
            out[j*4]   = (uint8_t)(c.state[j] >> 24);
            out[j*4+1] = (uint8_t)(c.state[j] >> 16);
            out[j*4+2] = (uint8_t)(c.state[j] >> 8);
            out[j*4+3] = (uint8_t)(c.state[j]);
        }
    }

    void hmac_sha256(const uint8_t* key, size_t key_len,
                     const uint8_t* msg, size_t msg_len,
                     uint8_t out[32]) {
        uint8_t k_block[64] = {0};
        if (key_len > 64) {
            Sha256Ctx kc; sha256_init(kc);
            sha256_update(kc, key, key_len);
            sha256_final(kc, k_block);
        } else {
            memcpy(k_block, key, key_len);
        }
        uint8_t ipad[64], opad[64];
        for (int i = 0; i < 64; i++) { ipad[i] = k_block[i] ^ 0x36; opad[i] = k_block[i] ^ 0x5c; }

        uint8_t inner[32];
        Sha256Ctx c1; sha256_init(c1);
        sha256_update(c1, ipad, 64);
        sha256_update(c1, msg, msg_len);
        sha256_final(c1, inner);

        Sha256Ctx c2; sha256_init(c2);
        sha256_update(c2, opad, 64);
        sha256_update(c2, inner, 32);
        sha256_final(c2, out);
    }

    void hmac_sha256_trunc4(const uint8_t* key, size_t key_len,
                            const uint8_t* msg, size_t msg_len,
                            uint8_t out_trunc[4]) {
        uint8_t full[32];
        hmac_sha256(key, key_len, msg, msg_len, full);
        memcpy(out_trunc, full, 4);
    }

    // Re-serializes the fields UavCmdPacket already extracted back into the
    // 28-byte wire layout HMAC was computed over (msg_code[1] + sequence[2]
    // + vehicle_hash[8] + issued_at_mod[4] + body[13]). Byte-identical to
    // hashing the original raw frame directly, since parsing preserved
    // every field's exact value and order.
    void reserialize_28(const UavCmdPacket& pkt, uint8_t out[28]) {
        out[0] = pkt.msg_code;
        out[1] = (uint8_t)((pkt.sequence >> 8) & 0xFF);
        out[2] = (uint8_t)(pkt.sequence & 0xFF);
        memcpy(out + 3, pkt.vehicle_hash, 8);
        out[11] = (uint8_t)((pkt.issued_at_mod >> 24) & 0xFF);
        out[12] = (uint8_t)((pkt.issued_at_mod >> 16) & 0xFF);
        out[13] = (uint8_t)((pkt.issued_at_mod >> 8)  & 0xFF);
        out[14] = (uint8_t)(pkt.issued_at_mod & 0xFF);
        memcpy(out + 15, pkt.body, 13);
    }

    bool verify_hmac(const UavCmdPacket& pkt) {
        if (s_hmac_key_len == 0) return false;
        uint8_t msg[28];
        reserialize_28(pkt, msg);
        uint8_t computed[4];
        hmac_sha256_trunc4(s_hmac_key, s_hmac_key_len, msg, sizeof(msg), computed);
        return memcmp(computed, pkt.hmac_trunc, 4) == 0;
    }

    // ------------------------------------------------------------------
    // Replay guard — ported from drone-companion's stanag_replay_check().
    // Sequence is monotonic per-vehicle (rolls over at 2^16); a legitimate
    // reset (e.g. relay reboot) is allowed through if issued_at_mod has
    // advanced by more than the replay window, even if sequence went
    // backwards. First frame seen is always accepted (bootstraps state).
    // ------------------------------------------------------------------
    #define UAV_REPLAY_WINDOW_S 60u   // matches drone-companion's default

    uint16_t s_last_seq = 0;
    uint32_t s_last_mod = 0;
    bool     s_have_seen_seq = false;

    bool replay_check(uint16_t sequence, uint32_t issued_at_mod) {
        if (!s_have_seen_seq) {
            s_last_seq = sequence;
            s_last_mod = issued_at_mod;
            s_have_seen_seq = true;
            return true;
        }
        int32_t delta = (int32_t)(int16_t)(sequence - s_last_seq);
        if (delta <= 0) {
            int32_t tdelta = (int32_t)(issued_at_mod - s_last_mod);
            if (tdelta < (int32_t)UAV_REPLAY_WINDOW_S) return false;
        }
        s_last_seq = sequence;
        s_last_mod = issued_at_mod;
        return true;
    }

    // Stubbed MAVLink-compatible command emission: we frame it as a simple
    // ASCII-decorated binary record so a PC-side ArduPilot-compatible bridge
    // can translate further. A full libmavlink build is deferred to T2.
    void emit_mavlink_command(uint8_t stanag_code, const DecodedBody& d) {
        uint8_t frame[32];
        frame[0] = MAVLINK_STX_V2;
        frame[1] = 20;                              // payload len
        frame[2] = 0; frame[3] = 0; frame[4] = 0;   // incompat/compat flags
        frame[5] = s_seq_counter++;
        frame[6] = MAV_SYSID_GCS;
        frame[7] = MAV_COMPID_UCS;
        frame[8]  = stanag_code;                    // reuse short code as "msgid" byte
        frame[9]  = 0; frame[10] = 0;
        // payload
        memcpy(&frame[11], &d.lat_e7, 4);
        memcpy(&frame[15], &d.lon_e7, 4);
        memcpy(&frame[19], &d.alt_m,  2);
        memcpy(&frame[21], &d.heading_x10, 2);
        frame[23] = d.raw_first_byte;
        // zero pad
        for (size_t i = 24; i < 30; i++) frame[i] = 0;
        uint16_t crc = crc16_mavlink(&frame[1], 29);
        frame[30] = (uint8_t)(crc & 0xFF);
        frame[31] = (uint8_t)((crc >> 8) & 0xFF);
        s_drone_uart.write(frame, sizeof(frame));
    }
} // namespace
#endif  // PDL_ENABLE_UAV_RELAY


// ── Public API ─────────────────────────────────────────────────────

void uav_relay_init(const uint8_t* hmac_key, size_t hmac_key_len) {
#ifdef PDL_ENABLE_UAV_RELAY
    // Load any NVS-persisted HMAC root from a prior pairing (see
    // uav_pairing.h) and open the pairing window on cold boot if none is
    // persisted yet. now_ms comes from millis() on-device; native
    // UNIT_TEST builds of this module use 0 since there's no prior-boot
    // NVS to load anyway (uav_pairing_init's cold-boot window still opens
    // correctly either way).
#ifdef ARDUINO
    uav_pairing_init(millis());
#else
    uav_pairing_init(0);
#endif
    uint8_t persisted_root[32];
    if (uav_pairing_get_hmac_root(persisted_root)) {
        memcpy(s_hmac_key, persisted_root, 32);
        s_hmac_key_len = 32;
    }
    // Explicit caller-supplied key (e.g. test injection) still works exactly
    // as before and takes priority over whatever NVS provided.
    if (hmac_key != nullptr && hmac_key_len > 0 && hmac_key_len <= sizeof(s_hmac_key)) {
        memcpy(s_hmac_key, hmac_key, hmac_key_len);
        s_hmac_key_len = hmac_key_len;
    }
    // UART to drone modem: TX=GPIO17, RX=GPIO18, 115200 (ArduPilot default)
    s_drone_uart.begin(115200, SERIAL_8N1, /*rx=*/18, /*tx=*/17);
    s_initialized = true;
#else
    (void)hmac_key; (void)hmac_key_len;
#endif
}

void uav_relay_set_own_sensor_id(uint32_t sensor_id) {
#ifdef PDL_ENABLE_UAV_RELAY
    s_own_sensor_id_hint = sensor_id;
#else
    (void)sensor_id;
#endif
}

void uav_relay_handle_pair_request(const uint8_t* data, size_t len) {
#ifdef PDL_ENABLE_UAV_RELAY
    // This sensor's own identity as the "drone" side of the handshake.
    // Uses s_own_sensor_id_hint (set via uav_relay_set_own_sensor_id(),
    // called once from main.cpp with enrollment_get_sensor_id()) rather
    // than depending on enrollment.cpp directly from this module -- same
    // "inject the id, don't query enrollment from every frame-handling
    // module" convention command_handler.cpp already uses (s_own_sensor_id
    // there), which also keeps this module's existing native-test
    // isolation (HardwareSerial stub, no real NVS/Arduino deps) intact.
    uint8_t vehicle_hash[8] = {0};
    memcpy(vehicle_hash, &s_own_sensor_id_hint, sizeof(s_own_sensor_id_hint));

    uint8_t response[UAV_PAIR_RSP_FRAME_SIZE];
    size_t  response_len = 0;
#ifdef ARDUINO
    uint32_t now_ms = millis();
#else
    uint32_t now_ms = 0;
#endif
    bool have_response = uav_pairing_process_request(data, len, now_ms, vehicle_hash,
                                                      response, &response_len);
    if (!have_response || response_len == 0) return;

    mesh_publish_raw(response, response_len);

    // Pairing may have just completed -- refresh this module's own runtime
    // key immediately so the very next command from the operator verifies
    // correctly without needing a reboot.
    uint8_t fresh_root[32];
    if (uav_pairing_get_hmac_root(fresh_root)) {
        memcpy(s_hmac_key, fresh_root, 32);
        s_hmac_key_len = 32;
    }
#else
    (void)data; (void)len;
#endif
}


bool uav_relay_parse(const uint8_t* data, size_t len, UavCmdPacket& out) {
#ifdef PDL_ENABLE_UAV_RELAY
    if (len != UAV_CMD_FRAME_SIZE + 1) {
        // The wire format includes the MSG_TYPE prefix byte (0x0A),
        // but some callers strip it. Accept either.
        if (len != UAV_CMD_FRAME_SIZE) return false;
    }
    const uint8_t* p = (len == UAV_CMD_FRAME_SIZE + 1) ? data + 1 : data;
    out.msg_code      = p[0];
    out.sequence      = ((uint16_t)p[1] << 8) | (uint16_t)p[2];
    memcpy(out.vehicle_hash, &p[3], 8);
    out.issued_at_mod = ((uint32_t)p[11] << 24) | ((uint32_t)p[12] << 16) |
                        ((uint32_t)p[13] << 8)  |  (uint32_t)p[14];
    memcpy(out.body,       &p[15], 13);
    memcpy(out.hmac_trunc, &p[28], 4);
    return true;
#else
    (void)data; (void)len; (void)out;
    return false;
#endif
}


void uav_relay_handle(const uint8_t* data, size_t len) {
#ifdef PDL_ENABLE_UAV_RELAY
    if (!s_initialized) return;

    UavCmdPacket pkt{};
    if (!uav_relay_parse(data, len, pkt)) {
        uav_relay_send_ack(0, UavAckCode::BAD_CRC);
        return;
    }

    // HMAC verification. BUG FIX: this was previously a documented no-op --
    // every frame passed through regardless of its hmac_trunc content as
    // long as SOME key had been configured (which nothing in main.cpp ever
    // did, so it was unconditionally bypassed either way). Now real:
    // verify_hmac() actually computes HMAC-SHA256(key, bytes[0..28])[0..4]
    // (ported from drone-companion's proven reference implementation, the
    // same wire spec both sides already share) and rejects on mismatch.
    // No key configured -> cannot verify -> reject non-emergency commands
    // too now that verification is real (previously only EMERGENCY_COMMAND
    // was gated on key presence; every other command type was unauthenticated
    // by design). A relay with no key configured is not yet provisioned for
    // secure operation.
    if (s_hmac_key_len == 0) {
        uav_relay_send_ack(pkt.sequence, UavAckCode::POLICY_BLOCK);
        return;
    }
    if (!verify_hmac(pkt)) {
        uav_relay_send_ack(pkt.sequence, UavAckCode::BAD_HMAC);
        return;
    }

    // Replay guard (new -- drone-companion has this, T1's relay didn't).
    // A valid-HMAC frame that's a stale/replayed sequence must still be
    // rejected; without this, a captured valid command could be re-sent
    // indefinitely. No dedicated ack code for "replay" exists in the
    // shared wire protocol (UavAckCode has no REPLAY value) -- reusing
    // BAD_HMAC, since both mean "this command's authenticity/freshness
    // could not be established."
    if (!replay_check(pkt.sequence, pkt.issued_at_mod)) {
        uav_relay_send_ack(pkt.sequence, UavAckCode::BAD_HMAC);
        return;
    }

    // Dispatch by short code.
    switch (pkt.msg_code) {
        case UAV_MSG_WAYPOINT_LOAD:
        case UAV_MSG_WAYPOINT_EXECUTE:
        case UAV_MSG_VEHICLE_STEERING_CMD:
        case UAV_MSG_POSITION_LLA:
        case UAV_MSG_EMERGENCY_COMMAND:
        case UAV_MSG_VSM_DATALINK_ASSIGN:
        case UAV_MSG_LOI_REQUEST: {
            DecodedBody d = decode_body(pkt.body);
            emit_mavlink_command(pkt.msg_code, d);
            uav_relay_send_ack(pkt.sequence, UavAckCode::OK);
            return;
        }
        default:
            uav_relay_send_ack(pkt.sequence, UavAckCode::UNKNOWN_TYPE);
            return;
    }
#else
    (void)data; (void)len;
#endif
}


void uav_relay_send_ack(uint16_t sequence, UavAckCode code) {
#ifdef PDL_ENABLE_UAV_RELAY
    uint8_t ack[UAV_ACK_FRAME_SIZE];
    ack[0] = MSG_TYPE_UAV_ACK;
    ack[1] = (uint8_t)((sequence >> 8) & 0xFF);
    ack[2] = (uint8_t)(sequence & 0xFF);
    ack[3] = (uint8_t)code;
    for (size_t i = 4; i < UAV_ACK_FRAME_SIZE; i++) ack[i] = 0;
    // Truncated CRC over payload
    uint16_t crc = 0;
    for (size_t i = 0; i < UAV_ACK_FRAME_SIZE - 2; i++) crc = (crc << 1) ^ ack[i];
    ack[UAV_ACK_FRAME_SIZE - 2] = (uint8_t)(crc & 0xFF);
    ack[UAV_ACK_FRAME_SIZE - 1] = (uint8_t)((crc >> 8) & 0xFF);
    // NOTE (found while building the telemetry uplink, not fixed here):
    // this CRC is neither the algorithm nor the byte order drone-companion's
    // ack_crc16()/ack_parse() actually use (real CRC-16-CCITT/XMODEM,
    // poly 0x1021 init 0, big-endian on the wire -- see uav_relay_crc16_
    // xmodem() below, which the NEW telemetry code uses correctly). This
    // ACK frame would not currently validate against drone-companion's own
    // ack_parse(). Flagged, not changed here -- out of scope for the
    // telemetry task and touches an already-shipped (if inert -- this
    // module isn't wired into Spencer01's build) wire format.
    mesh_publish_raw(ack, sizeof(ack));
#else
    (void)sequence; (void)code;
#endif
}


// ---------------------------------------------------------------------------
// Telemetry uplink (drone -> mesh) -- see uav_relay.h for design notes.
// ---------------------------------------------------------------------------
#ifdef PDL_ENABLE_UAV_RELAY
namespace {
    // CRC-16-CCITT (XMODEM variant): poly 0x1021, init 0x0000, no reflect.
    // Matches drone-companion/src/ack.cpp's ack_crc16() exactly -- the
    // telemetry beacon's CRC uses the same algorithm (SPEC.md 2.3).
    uint16_t crc16_xmodem(const uint8_t* data, size_t len) {
        uint16_t crc = 0;
        for (size_t i = 0; i < len; i++) {
            crc ^= (uint16_t)data[i] << 8;
            for (int b = 0; b < 8; b++) {
                crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
            }
        }
        return crc;
    }
} // namespace
#endif

bool uav_relay_parse_telemetry(const uint8_t* data, size_t len, UavTelemetryBeacon& out) {
#ifdef PDL_ENABLE_UAV_RELAY
    if (data == nullptr || len != UAV_TELEM_FRAME_SIZE) return false;
    if (data[0] != MSG_TYPE_UAV_TELEM) return false;

    uint16_t rx_crc = ((uint16_t)data[30] << 8) | (uint16_t)data[31];
    if (rx_crc != crc16_xmodem(data, 30)) return false;

    out.sequence = ((uint16_t)data[1] << 8) | (uint16_t)data[2];
    memcpy(out.vehicle_hash, &data[3], 8);
    out.issued_at_mod = ((uint32_t)data[11] << 24) | ((uint32_t)data[12] << 16) |
                        ((uint32_t)data[13] << 8)  |  (uint32_t)data[14];
    out.lat_e7 = (int32_t)(((uint32_t)data[15] << 24) | ((uint32_t)data[16] << 16) |
                           ((uint32_t)data[17] << 8)  |  (uint32_t)data[18]);
    out.lon_e7 = (int32_t)(((uint32_t)data[19] << 24) | ((uint32_t)data[20] << 16) |
                           ((uint32_t)data[21] << 8)  |  (uint32_t)data[22]);
    out.alt_m = (int16_t)(((uint16_t)data[23] << 8) | (uint16_t)data[24]);
    out.battery_mv = ((uint16_t)data[25] << 8) | (uint16_t)data[26];
    out.battery_pct = data[27];
    out.flight_mode = data[28];
    out.armed_flags = data[29];
    return true;
#else
    (void)data; (void)len; (void)out;
    return false;
#endif
}

void uav_relay_handle_telemetry(const uint8_t* data, size_t len) {
#ifdef PDL_ENABLE_UAV_RELAY
    // Deliberately NOT gated on s_initialized/HMAC: telemetry is periodic
    // status broadcast (not an actuation command), CRC-checked but not
    // HMAC-authenticated -- matches drone-companion's own design (SPEC.md
    // 2.3 documents no HMAC field on this frame, only crc16). Relaying it
    // does not require the STANAG command channel to be provisioned.
    UavTelemetryBeacon beacon{};
    if (!uav_relay_parse_telemetry(data, len, beacon)) return;

    // Relay verbatim onto T1's own mesh-facing link so a T2 gateway
    // (also a mesh peer) receives it -- same pattern as uav_relay_send_ack()
    // and every other cross-hop relay in this codebase (mesh_publish_raw).
    mesh_publish_raw(data, len);
#else
    (void)data; (void)len;
#endif
}
