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

// Mirrors dot11p.c / geonet.c's constants and layout, in reverse. Keep these two files in sync
// if either the TX-side frame shape or these constants change.
#define GN_ETHERTYPE                (0x8947)
#define LLC_SNAP_HEADER_LEN         (8)
#define IEEE80211_HEADER_LEN        (24)  // non-QoS Data
#define IEEE80211_QOS_CTRL_LEN      (2)   // extra field QoS Data frames add
#define IEEE80211_FC_TYPE_DATA      (2)
#define IEEE80211_FC_QOS_SUBTYPE_BIT (0x08)

#define GN_BASIC_HEADER_LEN         (4)
#define GN_COMMON_HEADER_LEN        (8)
#define BTP_B_HEADER_LEN            (4)

// Extended-header lengths per GeoNetworking header type - see gn_unwrap.h for why these exact
// numbers, and why they must not be assumed equal.
#define GN_SHB_EXT_HEADER_LEN       (28) // SO PV (24) + Reserved (4)
#define GN_GBC_EXT_HEADER_LEN       (44) // SN(2) + Rsvd(2) + SO PV(24) + area(12) + Rsvd(4)

// Offsets of the destination-area fields within the GBC extended header.
#define GBC_AREA_LAT_OFFSET         (28)
#define GBC_AREA_LON_OFFSET         (32)
#define GBC_AREA_DIST_A_OFFSET      (36)

#define GN_HEADER_TYPE_GBC          (4)  // GeoBroadcast
#define GN_HEADER_TYPE_TSB          (5)  // Topologically-Scoped Broadcast
#define GN_HEADER_SUBTYPE_SINGLE_HOP (0)

#define GN_NEXT_HEADER_COMMON       (1)  // unsecured: the Common Header follows
#define GN_NEXT_HEADER_SECURED      (2)  // a TS 103 097 envelope follows, Common Header inside it
#define GN_COMMON_NEXT_HEADER_BTP_B (2)

// Common Header field (clause 9.7): length of everything after the GeoNetworking headers, i.e.
// the BTP-B header plus the ITS payload.
#define GN_COMMON_PAYLOAD_LEN_OFFSET (4)

// IEEE 1609.2 / TS 103 097 envelope, COER encoded - see unwrap_secured().
#define IEEE1609DOT2_VERSION        (3)
#define CONTENT_TAG_UNSECURED_DATA  (0x80) // Ieee1609Dot2Content CHOICE, context tag 0
#define CONTENT_TAG_SIGNED_DATA     (0x81) //                             context tag 1
#define SIGNED_PAYLOAD_HAS_DATA     (0x40) // SignedDataPayload preamble: `data` present

#define BTP_DEST_PORT_CAM           (2001) // ETSI TS 103 248
#define BTP_DEST_PORT_DENM          (2002)
// NOTE the crossover: SPATEM is BTP port 2004 but ItsPduHeader messageID 4, while MAPEM is port
// 2003 and messageID 5. Port and messageID are NOT the same number - mixing them up routes every
// message to the wrong decoder on the phone.
#define BTP_DEST_PORT_SPATEM        (2004)

static const uint8_t s_llc_snap_prefix[6] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};

static int32_t be32(const uint8_t *p)
{
    return (int32_t)(((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) |
                     ((uint32_t)p[2] << 8) | (uint32_t)p[3]);
}

static uint16_t be16(const uint8_t *p)
{
    return (uint16_t)(((uint16_t)p[0] << 8) | (uint16_t)p[1]);
}

// COER length determinant (ITU-T X.696): a first byte below 0x80 is the length itself; otherwise
// its low 7 bits count the big-endian length bytes that follow. Two of them cover anything this
// radio can deliver. Returns how many bytes the determinant occupies, or 0 if it does not fit in
// `avail` or uses a form this does not read.
static int coer_length(const uint8_t *p, int avail, int *len)
{
    if (avail < 1) {
        return 0;
    }
    if (p[0] < 0x80) {
        *len = p[0];
        return 1;
    }
    const int n = p[0] & 0x7F;
    if (n < 1 || n > 2 || avail < 1 + n) {
        return 0;
    }
    int v = 0;
    for (int i = 1; i <= n; i++) {
        v = (v << 8) | p[i];
    }
    *len = v;
    return 1 + n;
}

// Locates the GeoNetworking packet inside a secured one. `offset` points just past the Basic
// Header. Returns the offset of the inner Common Header and sets *inner_end to where the envelope
// says the inner packet ends - which lies beyond frame_len if the capture was cut short - or
// returns -1 for anything this does not unwrap.
//
// The envelope is an Ieee1609Dot2Data (IEEE 1609.2, profiled by TS 103 097 v1.3.1 and later),
// COER encoded. A signed message starts:
//
//   03            protocolVersion 3
//   81            content = signedData
//   00            hashId (sha256; any one-byte value is accepted - the hash is not checked)
//   40            tbsData.payload preamble: `data` present (bit 6)
//   03 80 <len>   payload.data: an Ieee1609Dot2Data holding unsecuredData of <len> bytes, which
//                 are the Common Header, extended header, BTP-B header and ITS payload
//   ...           headerInfo, signer, signature: not read
//
// The inner packet comes first inside tbsData, so it is found without parsing the certificate
// or the signature, and its explicit length is what separates it from them. The shape is
// measured, not only read from the standard: all 157 signed frames in
// capture_20260817_171055.pcap have it (150 CAM, 7 GeoBroadcast DENM; <len> in all three COER
// forms), and asn1tools decodes every one of them to the same unsecuredData. A top-level
// unsecuredData (03 80 <len>, no signature at all) is accepted too.
static int unwrap_secured(const uint8_t *frame, int offset, int frame_len,
                          int *inner_end, bool *is_signed)
{
    const uint8_t *p = frame + offset;
    const int avail = frame_len - offset;
    int i;

    if (avail < 2 || p[0] != IEEE1609DOT2_VERSION) {
        return -1; // includes the legacy TS 103 097 v1.2.1 envelope, protocolVersion 2
    }
    if (p[1] == CONTENT_TAG_SIGNED_DATA) {
        if (avail < 6 ||
            p[2] >= 0x80 ||                        // hashId: a one-byte enumerated value
            !(p[3] & SIGNED_PAYLOAD_HAS_DATA) ||   // signs only a hash of data sent elsewhere
            p[4] != IEEE1609DOT2_VERSION ||
            p[5] != CONTENT_TAG_UNSECURED_DATA) {  // nested signing or encryption
            return -1;
        }
        i = 6;
        *is_signed = true;
    } else if (p[1] == CONTENT_TAG_UNSECURED_DATA) {
        i = 2;
        *is_signed = false;
    } else {
        return -1; // encryptedData, certificate requests
    }

    int len;
    const int used = coer_length(p + i, avail - i, &len);
    if (used == 0) {
        return -1;
    }
    i += used;
    *inner_end = offset + i + len;
    return offset + i;
}

bool gn_unwrap_its(const uint8_t *frame, int frame_len, gn_rx_t *out)
{
    if (!frame || !out || frame_len < IEEE80211_HEADER_LEN) {
        return false;
    }
    memset(out, 0, sizeof(*out));

    uint8_t fc0 = frame[0];
    uint8_t fc1 = frame[1];
    uint8_t type = (fc0 >> 2) & 0x03;
    uint8_t subtype = (fc0 >> 4) & 0x0F;
    bool to_ds = fc1 & 0x01;
    bool from_ds = fc1 & 0x02;

    // Only plain broadcast Data frames, no WDS. Both QoS Data (what real ITS-G5 hardware sends,
    // 26-byte header) and non-QoS Data (24-byte, what our own TX currently builds) are accepted.
    if (type != IEEE80211_FC_TYPE_DATA || (to_ds && from_ds)) {
        return false;
    }

    int offset = IEEE80211_HEADER_LEN;
    if (subtype & IEEE80211_FC_QOS_SUBTYPE_BIT) {
        offset += IEEE80211_QOS_CTRL_LEN;
    }

    if (frame_len < offset + LLC_SNAP_HEADER_LEN) {
        return false;
    }
    if (memcmp(frame + offset, s_llc_snap_prefix, sizeof(s_llc_snap_prefix)) != 0) {
        return false;
    }
    if (be16(frame + offset + 6) != GN_ETHERTYPE) {
        return false;
    }
    offset += LLC_SNAP_HEADER_LEN;

    // ---- GN Basic Header (4 bytes) ----
    if (frame_len < offset + GN_BASIC_HEADER_LEN) {
        return false;
    }
    const uint8_t basic_next_header = frame[offset] & 0x0F;
    offset += GN_BASIC_HEADER_LEN;

    // The headers from here on must end before `limit`: the end of the frame, or for a secured
    // packet the end of the envelope's inner packet if that comes first.
    int limit = frame_len;
    int envelope_end = -1;
    if (basic_next_header == GN_NEXT_HEADER_SECURED) {
        offset = unwrap_secured(frame, offset, frame_len, &envelope_end, &out->signed_unverified);
        if (offset < 0) {
            return false;
        }
        if (envelope_end < limit) {
            limit = envelope_end;
        }
    } else if (basic_next_header != GN_NEXT_HEADER_COMMON) {
        return false;
    }

    // ---- GN Common Header (8 bytes) ----
    if (limit < offset + GN_COMMON_HEADER_LEN) {
        return false;
    }
    uint8_t next_header = (frame[offset + 0] >> 4) & 0x0F;
    uint8_t header_type = (frame[offset + 1] >> 4) & 0x0F;
    uint8_t header_subtype = frame[offset + 1] & 0x0F;
    const int gn_payload_len = be16(frame + offset + GN_COMMON_PAYLOAD_LEN_OFFSET);
    if (next_header != GN_COMMON_NEXT_HEADER_BTP_B) {
        return false;
    }
    offset += GN_COMMON_HEADER_LEN;

    // ---- Extended header: length depends on the header type ----
    int ext_len;
    bool is_gbc = false;
    if (header_type == GN_HEADER_TYPE_TSB && header_subtype == GN_HEADER_SUBTYPE_SINGLE_HOP) {
        ext_len = GN_SHB_EXT_HEADER_LEN;
    } else if (header_type == GN_HEADER_TYPE_GBC) {
        // Subtype selects the area shape (0 circle, 1 rectangle, 2 ellipse). All three carry the
        // same field layout - DistanceB and Angle are simply unused for a circle - so the length
        // is the same and we don't need to branch on it.
        ext_len = GN_GBC_EXT_HEADER_LEN;
        is_gbc = true;
    } else {
        return false; // Beacon / GeoUnicast / GeoAnycast / multi-hop TSB - see header comment
    }
    if (limit < offset + ext_len) {
        return false;
    }
    if (is_gbc) {
        out->has_geo_area = true;
        out->geo_area_lat_tenmicrodeg = be32(frame + offset + GBC_AREA_LAT_OFFSET);
        out->geo_area_lon_tenmicrodeg = be32(frame + offset + GBC_AREA_LON_OFFSET);
        out->geo_area_distance_a_m = be16(frame + offset + GBC_AREA_DIST_A_OFFSET);
    }
    offset += ext_len;

    // ---- BTP-B header (4 bytes) ----
    if (limit < offset + BTP_B_HEADER_LEN) {
        return false;
    }
    uint16_t dest_port = be16(frame + offset);
    if (dest_port != BTP_DEST_PORT_CAM && dest_port != BTP_DEST_PORT_DENM &&
        dest_port != BTP_DEST_PORT_SPATEM) {
        return false;
    }

    // ---- ITS payload: exactly as long as the Common Header declares ----
    // Not "whatever is left of the frame": see "Payload bounds" in gn_unwrap.h for the 8 trailing
    // bytes every received frame carries and the signature that follows a secured packet.
    if (gn_payload_len <= BTP_B_HEADER_LEN) {
        return false; // no ITS payload at all
    }
    const int payload_start = offset + BTP_B_HEADER_LEN;
    const int payload_end = offset + gn_payload_len;
    if (envelope_end >= 0 && payload_end > envelope_end) {
        return false; // the inner packet claims more than its envelope holds
    }
    out->truncated = payload_end > frame_len;
    const int payload_len = (out->truncated ? frame_len : payload_end) - payload_start;
    if (payload_len <= 0) {
        return false;
    }

    out->btp_dest_port = dest_port;
    out->payload = frame + payload_start;
    out->payload_len = payload_len;
    return true;
}