#include "geonet.h"
#include <string.h>

// GeoNetworking is big-endian throughout, unlike this project's serial framing.
static void put_be16(uint8_t **p, uint16_t v)
{
    *(*p)++ = (uint8_t)(v >> 8);
    *(*p)++ = (uint8_t)(v);
}

static void put_be32(uint8_t **p, uint32_t v)
{
    *(*p)++ = (uint8_t)(v >> 24);
    *(*p)++ = (uint8_t)(v >> 16);
    *(*p)++ = (uint8_t)(v >> 8);
    *(*p)++ = (uint8_t)(v);
}

int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
                     const gn_lpv_t *lpv,
                     uint16_t btp_dest_port,
                     uint8_t *out, size_t out_len)
{
    // GN Basic Header (4) + GN Common Header (8) + SHB extended header (28)
    // + BTP-B header (4) + ITS payload
    int total = 4 + 8 + 28 + 4 + its_len;
    if ((size_t)total > out_len) {
        return -1;
    }

    uint8_t *p = out;

    // ---- GN Basic Header (4 bytes) ---- (EN 302 636-4-1 clause 9.6)
    *p++ = (uint8_t)((1 << 4) | 1); // version=1, NextHeader=1 (Common Header, unsecured)
    *p++ = 0x00;                    // reserved
    // Lifetime: multiplier in the upper 6 bits, base in the lower 2 (0 = 50 ms, 1 = 1 s, 2 = 10 s,
    // 3 = 100 s). 0x05 = 1 x 1 s, which is what every other station in
    // its-g5-receiver-firmware/recordings sends its CAMs with. This was 0x83, commented as ~60 s but
    // decoding to 32 x 100 s = 3200 s, beyond the 600 s itsGnMaxPacketLifetime a sender may use at
    // all. A DENM would want a longer one (the captured GeoBroadcast DENMs use 0x79, 30 s).
    *p++ = 0x05;
    *p++ = 1;                       // remaining hop limit = 1 (SHB single-hop; matches CAM in the Rust reference)

    // ---- GN Common Header (8 bytes) ---- (clause 9.7)
    *p++ = (uint8_t)((2 << 4) | 0); // NextHeader=2 (BTP-B), reserved nibble
    // HeaderType=5 (TSB), HeaderSubtype=0 (SINGLE_HOP) per table 9 - this is
    // the actual encoding for single-hop broadcast. An earlier version of
    // this code used (2,0), which is GEOUNICAST - wrong header type entirely
    // for a broadcast frame; real receivers would try to match the
    // destination-address extended header GeoUnicast expects and mishandle
    // or reject the packet.
    *p++ = (uint8_t)((5 << 4) | 0);
    *p++ = 0x02;                    // traffic class: SCF=0, ChannelOffload=0, TC-ID=2 (clause 9.7.5)
    *p++ = 0x80;                    // flags: bit0 = "is mobile" station (clause 9.7.2)
    // Payload length = what follows the WHOLE GeoNetworking header
    // (Basic+Common+Extended), i.e. BTP-B header + ITS payload only - does
    // NOT include the 24-byte extended header itself. An earlier version of
    // this code wrongly added the 24 bytes in here too.
    uint16_t payload_len = (uint16_t)(4 + its_len);
    *p++ = (uint8_t)(payload_len >> 8);
    *p++ = (uint8_t)(payload_len & 0xFF);
    *p++ = 1;                       // max hop limit = 1, matches basic header RHL (SHB single-hop)
    *p++ = 0x00;                    // reserved

    // ---- SHB extended header: Source Position Vector (24) + Reserved (4) = 28 bytes ----
    // (clause 9.5.2). GN_ADDR (8 bytes) is itself structured, not a raw
    // pseudonym (clause 9.5.1): bit0 M-flag(0=auto-derived), bits1-5 ITS-S
    // type (5-bit), bits6-15 reserved(=0), then octets2-7 = MID, which is
    // defined to BE the link-layer (802.11) address - so this must match
    // the source address dot11p_build_frame uses, not just "look similar."
    uint8_t gn_addr[8];
    gn_addr[0] = (uint8_t)((0 << 7) | ((lpv->station_type & 0x1F) << 2)); // M=0, ST=station_type, top 2 reserved bits=0
    gn_addr[1] = 0x00; // remaining 8 reserved bits
    memcpy(&gn_addr[2], lpv->mac, 6); // MID = link-layer address
    memcpy(p, gn_addr, 8); p += 8;
    // TST (4 bytes): when the position below was acquired, ms, TimestampIts mod 2^32.
    put_be32(&p, lpv->tst_ms);
    // Latitude/Longitude (4+4 bytes, signed, 1/10 microdegree) - fixed-width binary fields, not
    // UPER bit-packed like the CAM payload's own position.
    put_be32(&p, (uint32_t)lpv->lat_tenmicrodeg);
    put_be32(&p, (uint32_t)lpv->lon_tenmicrodeg);
    // PAI (1 bit) + Speed (15 bits, signed, 0.01 m/s). Clamped, not masked: a 15-bit value that
    // overflows wraps its sign bit and reads as travelling backwards at speed.
    int32_t speed = lpv->speed_cms;
    if (speed > 16383) speed = 16383;
    if (speed < -16384) speed = -16384;
    put_be16(&p, (uint16_t)(((lpv->pai ? 1u : 0u) << 15) | ((uint16_t)speed & 0x7FFFu)));
    // Heading (16 bits, 0.1 degree from north, clockwise, 0..3599).
    put_be16(&p, (uint16_t)(lpv->heading_decideg % 3600u));
    // Reserved (4 bytes) - clause 9.8.4: the SHB extended header is the 24-byte Source Position
    // Vector FOLLOWED BY a 4-byte reserved field (media-dependent data), 28 bytes in total. These
    // four bytes were missing, which is why a standards-compliant receiver read our CAM payload's
    // first two bytes (0x02 0x02 = protocolVersion/messageID) as the BTP destination port and saw
    // 514 instead of 2001 - confirmed against live air capture, 2026-08-13. Our own gn_unwrap.c
    // had the identical off-by-four, so ESP32<->ESP32 worked and nothing else did.
    *p++ = 0x00; *p++ = 0x00; *p++ = 0x00; *p++ = 0x00;

    // ---- BTP-B header (4 bytes) ----
    *p++ = (uint8_t)(btp_dest_port >> 8);
    *p++ = (uint8_t)(btp_dest_port & 0xFF);
    *p++ = 0x00; *p++ = 0x00; // destination port info, unused for BTP-B

    // ---- ITS payload (CAM UPER bytes from the phone) ----
    memcpy(p, its_payload, its_len);
    p += its_len;

    return (int)(p - out);
}