// ============================================================================
// ble_odid_scanner.cpp — see ble_odid_scanner.h for design notes.
//
// Section 1 (pure parsing) is a direct port of the parsing logic from PDL
// Witness's ble_droneid.c (classify_vendor / parse_advert), restructured to
// fill OdidParseResult instead of raw pointers, plus new Location/Basic-ID
// extraction so decoded detections can carry the drone's *actual* broadcast
// position instead of only the sensor's own position.
//
// Section 2 (NimBLE-Arduino integration) is new — it attaches a passive scan
// to the NimBLE host T1 already brings up for ble_companion.cpp instead of
// bringing up its own, since only one module may own NimBLEDevice::init().
// ============================================================================

#include "ble_odid_scanner.h"
#include "handset_fingerprint.h"

#include <cstring>

// ============================================================================
// SECTION 1 — Pure parsing (no BLE-stack dependency)
// ============================================================================

// Bluetooth SIG AD type values (hardcoded rather than pulled from a NimBLE/
// ESP-IDF header so this section stays dependency-free for native tests).
#define AD_TYPE_SVC_DATA_UUID16   0x16
#define AD_TYPE_SVC_DATA_UUID128  0x21
#define AD_TYPE_MFG_DATA          0xFF

// OpenDroneID 16-bit Service UUID (legacy / BT4 form) + AppCode prefix.
#define ODID_SERVICE_UUID_16      0xFFFA
#define ODID_APP_CODE             0x0D

// Bluetooth SIG Company IDs (LSB-first on the wire).
#define COMPANY_DJI     0x09DB  // "SZ DJI Technology Co., Ltd."
#define COMPANY_PARROT  0x0067  // "Parrot S.A."
// Autel and Skydio do not have well-known SIG IDs in shipping RID adverts;
// fall back to the Basic-ID serial prefix for those (same as PDL Witness).

static uint8_t classify_vendor(int32_t company_id,
                                const uint8_t* odid, size_t odid_len) {
    if (company_id == COMPANY_DJI)    return ODID_VENDOR_DJI;
    if (company_id == COMPANY_PARROT) return ODID_VENDOR_PARROT;

    if (odid && odid_len >= 22) {
        uint8_t hdr = odid[0];
        if ((hdr & 0x0F) == 0x0) {
            const char* uas_id = (const char*)&odid[2];
            if (!memcmp(uas_id, "1581F", 5)) return ODID_VENDOR_DJI;
            if (!memcmp(uas_id, "0SH",   3)) return ODID_VENDOR_DJI;
            if (!memcmp(uas_id, "SK-",   3)) return ODID_VENDOR_SKYDIO;
            if (!memcmp(uas_id, "MD",    2)) return ODID_VENDOR_AUTEL;
            if (!memcmp(uas_id, "EVO",   3)) return ODID_VENDOR_AUTEL;
            if (!memcmp(uas_id, "PI",    2)) return ODID_VENDOR_PARROT;
        }
    }
    return ODID_VENDOR_UNKNOWN;
}

bool odid_extract_basic_id_serial(const uint8_t* odid, size_t odid_len, char out[21]) {
    if (!odid || odid_len < 22) return false;
    uint8_t hdr = odid[0];
    if ((hdr & 0x0F) != 0x0) return false;  // not Basic ID

    memcpy(out, &odid[2], 20);
    out[20] = '\0';
    // ASTM F3411 NUL/space-pads the 20-byte field; trim for a clean C-string.
    for (int i = 19; i >= 0 && (out[i] == '\0' || out[i] == ' '); i--) out[i] = '\0';
    return true;
}

bool odid_decode_location(const uint8_t* body, size_t body_len, OdidLocation* out) {
    out->valid = false;
    if (!body || body_len < 25) return false;
    uint8_t hdr = body[0];
    if ((hdr & 0x0F) != 0x1) return false;  // not a Location message

    uint16_t dir = (uint16_t)body[2] | ((uint16_t)(body[3] & 0x01) << 8);
    uint8_t  speed_raw = body[4];
    int32_t lat_raw = (int32_t)((uint32_t)body[6]  | ((uint32_t)body[7]  << 8) |
                                 ((uint32_t)body[8]  << 16) | ((uint32_t)body[9]  << 24));
    int32_t lon_raw = (int32_t)((uint32_t)body[10] | ((uint32_t)body[11] << 8) |
                                 ((uint32_t)body[12] << 16) | ((uint32_t)body[13] << 24));
    uint16_t geo_alt_raw = (uint16_t)body[16] | ((uint16_t)body[17] << 8);

    out->lat_deg     = (double)lat_raw / 1e7;
    out->lon_deg     = (double)lon_raw / 1e7;
    out->alt_m       = ((float)geo_alt_raw / 2.0f) - 1000.0f;
    out->speed_ms    = (float)speed_raw * 0.25f;
    out->heading_deg = dir;
    out->valid       = true;
    return true;
}

OdidParseResult odid_parse_advert(const uint8_t* data, size_t len) {
    OdidParseResult r{};
    r.hit = false;
    r.vendor = ODID_VENDOR_UNKNOWN;
    r.has_serial = false;
    r.serial[0] = '\0';
    r.location.valid = false;
    r.company_id = -1;

    const uint8_t* odid = nullptr;
    size_t odid_len = 0;

    if (!data) return r;

    size_t i = 0;
    while (i + 1 < len) {
        uint8_t field_len = data[i];
        if (field_len == 0) break;
        if (i + field_len >= len) break;
        uint8_t ad_type = data[i + 1];
        const uint8_t* ad_data = &data[i + 2];
        size_t ad_data_len = field_len - 1;

        switch (ad_type) {
        case AD_TYPE_SVC_DATA_UUID16: {
            if (ad_data_len >= 2) {
                uint16_t uuid = (uint16_t)ad_data[0] | ((uint16_t)ad_data[1] << 8);
                if (uuid == ODID_SERVICE_UUID_16 && ad_data_len >= 4) {
                    if (ad_data[2] == ODID_APP_CODE) {
                        odid = &ad_data[3];
                        odid_len = ad_data_len - 3;
                    } else {
                        odid = &ad_data[2];
                        odid_len = ad_data_len - 2;
                    }
                    r.hit = true;
                }
            }
            break;
        }
        case AD_TYPE_SVC_DATA_UUID128: {
            if (ad_data_len >= 17) {
                const uint8_t* body = &ad_data[16];
                size_t body_len = ad_data_len - 16;
                uint8_t mt = body[0] & 0x0F;
                if (mt <= 0x5 || mt == 0xF) {
                    odid = body;
                    odid_len = body_len;
                    r.hit = true;
                }
            }
            break;
        }
        case AD_TYPE_MFG_DATA: {
            if (ad_data_len >= 2) {
                int32_t cid = (int32_t)ad_data[0] | ((int32_t)ad_data[1] << 8);
                r.company_id = cid;
                if (cid == COMPANY_DJI && ad_data_len >= 3) {
                    const uint8_t* body = &ad_data[2];
                    size_t body_len = ad_data_len - 2;
                    uint8_t mt = body[0] & 0x0F;
                    if (mt <= 0x5 || mt == 0xF) {
                        odid = body;
                        odid_len = body_len;
                    }
                    r.hit = true;
                }
            }
            break;
        }
        default:
            break;
        }

        i += 1 + field_len;
    }

    if (!r.hit) return r;

    r.vendor = classify_vendor(r.company_id, odid, odid_len);
    r.has_serial = odid_extract_basic_id_serial(odid, odid_len, r.serial);
    odid_decode_location(odid, odid_len, &r.location);
    return r;
}

// ============================================================================
// SECTION 2 — NimBLE-Arduino integration (real hardware only)
// ============================================================================
#ifdef PDL_BLE_COMPANION_ENABLED

#include <NimBLEDevice.h>
#include "config.h"
#include "blue_force.h"
#include "ffps.h"
#include "enrollment.h"
#include "sensor_location.h"
#include "health_report.h"

// Scan timing — 100% duty (interval == window), passive. Matches PDL Witness.
#define ODID_SCAN_INTERVAL_MS  100
#define ODID_SCAN_WINDOW_MS    100

static OdidScannerStats s_stats{};
static bool s_initialized = false;

// Shared by the real onResult callback AND ble_odid_scanner_test_inject() —
// so an on-device test that injects a synthetic payload exercises the exact
// same detection-building/blue-force/FFPS logic a real advert would, not a
// re-implementation of it.
static void process_hit(const OdidParseResult& r, int8_t rssi) {
    s_stats.rid_hits++;
    if (r.location.valid) s_stats.location_hits++;

    DetectionEvent evt;
    memset(&evt, 0, sizeof(evt));
    evt.msg_type      = MSG_TYPE_CANDIDATE;
    evt.sensor_id     = enrollment_get_sensor_id();
    evt.timestamp_utc = (uint32_t)(millis() / 1000);
    evt.class_label   = CLASS_RID_BLE_DECODED;
    evt.confidence    = 255;  // decoded broadcast content, not a probabilistic guess
    evt.rssi          = (int16_t)rssi;
    evt.noise_floor   = 0;    // not meaningful for a BLE advert
    evt.battery_pct   = health_read_battery_pct();
    evt.flags         = 0;

    if (r.location.valid) {
        // Real decoded drone position — a genuine upgrade over the RF
        // classifier path, which can only report the sensor's own
        // location (there's no way to derive drone position from
        // spectral shape alone).
        evt.lat_fixed = (int32_t)(r.location.lat_deg * 1e7);
        evt.lon_fixed = (int32_t)(r.location.lon_deg * 1e7);
    } else {
        // No Location message in this particular advert (drones cycle
        // Basic ID / Location / System across separate adverts) — fall
        // back to the sensor's own position, same as the RF path.
        location_get(&evt.lat_fixed, &evt.lon_fixed);
    }

    if (r.has_serial) {
        blue_force_compute_hash(r.serial, evt.signature_hash);
        // Carry the decoded drone Remote-ID serial so the C2 map can identify
        // the specific aircraft (direct-WiFi path forwards it as platform_id).
        strncpy(evt.drone_serial, r.serial, sizeof(evt.drone_serial) - 1);
        evt.drone_serial[sizeof(evt.drone_serial) - 1] = '\0';
    }

    // Same policy as the WiFi RF classifier path in main.cpp: check
    // blue-force before FFPS so a friendly emitter never even enters
    // the corroboration window.
    if (r.has_serial && blue_force_check_hash(evt.signature_hash)) {
        evt.flags |= FLAG_BLUE_FORCE;
        s_stats.blue_force_matches++;
    } else {
        ffps_submit_candidate(evt);
    }
}

class OdidScanCallbacks : public NimBLEAdvertisedDeviceCallbacks {
public:
    void onResult(NimBLEAdvertisedDevice* dev) override {
        s_stats.adverts_seen++;

        uint8_t* payload = dev->getPayload();
        size_t   payload_len = dev->getPayloadLength();
        if (!payload || payload_len == 0) return;

        // General passive handset/phone metadata (OUI/company-ID vendor
        // match) — runs on every advert regardless of ODID hit, same single
        // NimBLE scan-callback slot (only one registration is supported).
        // NimBLEAddress::getNative() returns bytes in REVERSED order vs.
        // standard MAC notation (confirmed against NimBLEAddress.cpp's own
        // toString(), which reverses before printing) — reverse back here
        // so mac[0..2] is the OUI, matching handset_fingerprint's lookup.
        {
            // NimBLEAddress must outlive the getNative() pointer — a
            // temporary here would be destroyed at the end of this
            // statement, leaving raw dangling before it's ever read.
            NimBLEAddress addr = dev->getAddress();
            const uint8_t* raw = addr.getNative();
            uint8_t mac[6] = {raw[5], raw[4], raw[3], raw[2], raw[1], raw[0]};
            bool is_public = (addr.getType() == BLE_ADDR_PUBLIC);
            handset_fingerprint_process_ble_adv(mac, payload, payload_len,
                                                (int8_t)dev->getRSSI(), is_public);
        }

        OdidParseResult r = odid_parse_advert(payload, payload_len);
        if (!r.hit) return;

        process_hit(r, (int8_t)dev->getRSSI());
    }
};

static OdidScanCallbacks s_callbacks;

// Test-only injection: parses a synthetic BLE advert payload through the
// EXACT same odid_parse_advert() + process_hit() path a real over-the-air
// advert would take. Lets an on-device test prove the full production
// pipeline (parse -> classify -> blue-force check -> FFPS submit) actually
// works on real hardware without needing a second radio to broadcast with.
bool ble_odid_scanner_test_inject(const uint8_t* payload, size_t len, int8_t rssi) {
    s_stats.adverts_seen++;
    OdidParseResult r = odid_parse_advert(payload, len);
    if (!r.hit) return false;
    process_hit(r, rssi);
    return true;
}

void ble_odid_scanner_init() {
    if (s_initialized) return;

    NimBLEScan* scan = NimBLEDevice::getScan();
    scan->setAdvertisedDeviceCallbacks(&s_callbacks, /*wantDuplicates=*/true);
    scan->setActiveScan(false);   // passive — never request a scan response
    scan->setInterval(ODID_SCAN_INTERVAL_MS);
    scan->setWindow(ODID_SCAN_WINDOW_MS);
    scan->setDuplicateFilter(false);  // want every advert for RSSI tracking

    scan->start(0 /* duration=0 -> scan indefinitely */, nullptr, false);

    s_initialized = true;
    Serial.println("[OdidScanner] BT5 LE passive scan started (100ms/100ms, 100% duty)");
}

void ble_odid_scanner_get_stats(OdidScannerStats* out) {
    if (out) *out = s_stats;
}

#endif // PDL_BLE_COMPANION_ENABLED
