// ============================================================================
// Native Unity tests — ble_odid_scanner.cpp (pure parsing section only)
//
//   pio test -e native -f test_ble_odid
//
// Strategy: include ble_odid_scanner.cpp directly as a single TU (same
// pattern as test_wfx_features / test_command_handler). PDL_BLE_COMPANION_ENABLED
// is not defined in the native env, so only Section 1 (pure parsing, zero
// Arduino/NimBLE dependency) compiles — Section 2 (NimBLE integration) is
// entirely #ifdef'd out, exactly as intended.
// ============================================================================

#include <unity.h>
#include <cstdint>
#include <cstring>
#include <cmath>

#include "../../src/ble_odid_scanner.cpp"

// ---------------------------------------------------------------------------
// Helpers — build synthetic ASTM F3411 message bodies / BLE AD payloads,
// mirroring the exact wire format used by Downloads/rid-simulator.ino so a
// real simulator broadcast would decode identically.
// ---------------------------------------------------------------------------

// Header byte layout per the authoritative opendroneid.c reference
// (node-mode-dualcore/src/opendroneid.h: "Byte 0 [MessageType][ProtoVersion]
// -- must define LSb first" — MessageType is the LOW nibble, ProtoVersion
// the HIGH nibble). NOTE: Downloads/rid-simulator.ino encodes this backwards
// ((msg_type << 4) | version) — a real bug in that tool, found while writing
// these fixtures; flagged separately, not fixed here (out of scope tonight).
static uint8_t odid_header_byte(uint8_t msg_type, uint8_t proto_version) {
    return (uint8_t)((proto_version << 4) | (msg_type & 0x0F));
}

static void build_basic_id_message(uint8_t* buf, const char* serial) {
    memset(buf, 0, 25);
    buf[0] = odid_header_byte(0x0, 0x00);  // MSG_TYPE_BASIC_ID, version 0
    buf[1] = (0x01 << 4) | 0x02;     // ID Type: Serial Number | UA Type: Helicopter/Multirotor
    strncpy((char*)&buf[2], serial, 20);
}

static int32_t encode_lat_lon(double deg) { return (int32_t)(deg * 1e7); }
static uint16_t encode_alt(float alt_m) { return (uint16_t)((alt_m + 1000.0f) * 2.0f); }
static uint8_t encode_speed(float speed_ms) { return (uint8_t)(speed_ms * 4.0f); }

static void build_location_message(uint8_t* buf, double lat, double lon,
                                    float alt_m, float speed_ms, uint16_t heading) {
    memset(buf, 0, 25);
    buf[0] = odid_header_byte(0x1, 0x00);  // MSG_TYPE_LOCATION
    buf[1] = 0x02;               // Status: Airborne
    buf[2] = heading & 0xFF;
    buf[3] = (heading >> 8) & 0x01;
    buf[4] = encode_speed(speed_ms);
    buf[5] = 1;  // vertical speed

    int32_t lat_raw = encode_lat_lon(lat);
    buf[6] = lat_raw & 0xFF; buf[7] = (lat_raw >> 8) & 0xFF;
    buf[8] = (lat_raw >> 16) & 0xFF; buf[9] = (lat_raw >> 24) & 0xFF;

    int32_t lon_raw = encode_lat_lon(lon);
    buf[10] = lon_raw & 0xFF; buf[11] = (lon_raw >> 8) & 0xFF;
    buf[12] = (lon_raw >> 16) & 0xFF; buf[13] = (lon_raw >> 24) & 0xFF;

    uint16_t press_alt = encode_alt(alt_m);
    buf[14] = press_alt & 0xFF; buf[15] = (press_alt >> 8) & 0xFF;

    // odid_decode_location() reads the geodetic altitude field (not the
    // pressure altitude field) — encode the same alt_m here so the round
    // trip is exact for the test.
    uint16_t geo_alt = encode_alt(alt_m);
    buf[16] = geo_alt & 0xFF; buf[17] = (geo_alt >> 8) & 0xFF;
}

// Wrap an ODID message body in a Service-Data-UUID16 (0xFFFA) AD structure
// with the ASTM F3411 AppCode prefix (0x0D), the way a real BT4 legacy
// advert carries it. Returns total advert length written to out.
static size_t wrap_svc_data16(uint8_t* out, const uint8_t* odid_msg, size_t odid_len) {
    size_t i = 0;
    uint8_t field_len = (uint8_t)(1 /*ad_type*/ + 2 /*uuid*/ + 1 /*appcode*/ + odid_len);
    out[i++] = field_len;
    out[i++] = AD_TYPE_SVC_DATA_UUID16;
    out[i++] = ODID_SERVICE_UUID_16 & 0xFF;
    out[i++] = (ODID_SERVICE_UUID_16 >> 8) & 0xFF;
    out[i++] = ODID_APP_CODE;
    memcpy(&out[i], odid_msg, odid_len);
    i += odid_len;
    return i;
}

// Wrap an ODID message body as DJI manufacturer-specific data (AD type 0xFF).
static size_t wrap_dji_mfg_data(uint8_t* out, const uint8_t* odid_msg, size_t odid_len) {
    size_t i = 0;
    uint8_t field_len = (uint8_t)(1 /*ad_type*/ + 2 /*company id*/ + odid_len);
    out[i++] = field_len;
    out[i++] = AD_TYPE_MFG_DATA;
    out[i++] = COMPANY_DJI & 0xFF;
    out[i++] = (COMPANY_DJI >> 8) & 0xFF;
    memcpy(&out[i], odid_msg, odid_len);
    i += odid_len;
    return i;
}

// ---------------------------------------------------------------------------
// odid_decode_location
// ---------------------------------------------------------------------------

static void test_location_roundtrip_known_vector() {
    uint8_t msg[25];
    build_location_message(msg, 39.1200, -77.5580, 45.0f, 8.5f, 90);

    OdidLocation loc{};
    bool ok = odid_decode_location(msg, sizeof(msg), &loc);

    TEST_ASSERT_TRUE(ok);
    TEST_ASSERT_TRUE(loc.valid);
    TEST_ASSERT_FLOAT_WITHIN(1e-4f, 39.1200f, (float)loc.lat_deg);
    TEST_ASSERT_FLOAT_WITHIN(1e-4f, -77.5580f, (float)loc.lon_deg);
    TEST_ASSERT_FLOAT_WITHIN(0.5f, 45.0f, loc.alt_m);
    TEST_ASSERT_FLOAT_WITHIN(0.25f, 8.5f, loc.speed_ms);
    TEST_ASSERT_EQUAL_UINT16(90, loc.heading_deg);
}

static void test_location_rejects_wrong_message_type() {
    uint8_t msg[25];
    build_basic_id_message(msg, "FA-SIM-20260404");  // message_type 0x0, not Location

    OdidLocation loc{};
    bool ok = odid_decode_location(msg, sizeof(msg), &loc);

    TEST_ASSERT_FALSE(ok);
    TEST_ASSERT_FALSE(loc.valid);
}

static void test_location_rejects_short_buffer() {
    uint8_t msg[10] = {0};
    msg[0] = 0x10;  // header claims Location type but buffer is too short

    OdidLocation loc{};
    bool ok = odid_decode_location(msg, sizeof(msg), &loc);

    TEST_ASSERT_FALSE(ok);
}

// ---------------------------------------------------------------------------
// odid_extract_basic_id_serial
// ---------------------------------------------------------------------------

static void test_basic_id_serial_extracted_and_trimmed() {
    uint8_t msg[25];
    build_basic_id_message(msg, "FA-SIM-20260404");

    char serial[21];
    bool ok = odid_extract_basic_id_serial(msg, sizeof(msg), serial);

    TEST_ASSERT_TRUE(ok);
    TEST_ASSERT_EQUAL_STRING("FA-SIM-20260404", serial);
}

static void test_basic_id_rejects_wrong_message_type() {
    uint8_t msg[25];
    build_location_message(msg, 0, 0, 0, 0, 0);  // message_type 0x1, not Basic ID

    char serial[21];
    bool ok = odid_extract_basic_id_serial(msg, sizeof(msg), serial);

    TEST_ASSERT_FALSE(ok);
}

// ---------------------------------------------------------------------------
// odid_parse_advert — full AD-structure walk
// ---------------------------------------------------------------------------

static void test_parse_advert_svc_data16_basic_id() {
    uint8_t basic_id[25];
    build_basic_id_message(basic_id, "SK-TESTUNIT1234567");

    uint8_t advert[40];
    size_t advert_len = wrap_svc_data16(advert, basic_id, sizeof(basic_id));

    OdidParseResult r = odid_parse_advert(advert, advert_len);

    TEST_ASSERT_TRUE(r.hit);
    TEST_ASSERT_TRUE(r.has_serial);
    TEST_ASSERT_EQUAL_STRING("SK-TESTUNIT1234567", r.serial);
    TEST_ASSERT_EQUAL_UINT8(ODID_VENDOR_SKYDIO, r.vendor);
    TEST_ASSERT_FALSE(r.location.valid);
}

static void test_parse_advert_svc_data16_location() {
    uint8_t loc_msg[25];
    build_location_message(loc_msg, 39.1210, -77.5560, 50.0f, 10.2f, 70);

    uint8_t advert[40];
    size_t advert_len = wrap_svc_data16(advert, loc_msg, sizeof(loc_msg));

    OdidParseResult r = odid_parse_advert(advert, advert_len);

    TEST_ASSERT_TRUE(r.hit);
    TEST_ASSERT_FALSE(r.has_serial);
    TEST_ASSERT_TRUE(r.location.valid);
    TEST_ASSERT_FLOAT_WITHIN(1e-4f, 39.1210f, (float)r.location.lat_deg);
    TEST_ASSERT_FLOAT_WITHIN(1e-4f, -77.5560f, (float)r.location.lon_deg);
}

static void test_parse_advert_dji_manufacturer_data() {
    uint8_t basic_id[25];
    build_basic_id_message(basic_id, "1581F0000000000001");

    uint8_t advert[40];
    size_t advert_len = wrap_dji_mfg_data(advert, basic_id, sizeof(basic_id));

    OdidParseResult r = odid_parse_advert(advert, advert_len);

    TEST_ASSERT_TRUE(r.hit);
    TEST_ASSERT_EQUAL_INT32(COMPANY_DJI, r.company_id);
    TEST_ASSERT_EQUAL_UINT8(ODID_VENDOR_DJI, r.vendor);
    TEST_ASSERT_TRUE(r.has_serial);
}

static void test_parse_advert_non_odid_ignored() {
    // A generic 0xFF manufacturer-data advert from a non-DJI company ID with
    // a payload that doesn't parse as an ODID header — should not hit.
    uint8_t advert[8] = { 0x06, AD_TYPE_MFG_DATA, 0x4C, 0x00, 0x02, 0x15, 0xAA, 0xBB };
    OdidParseResult r = odid_parse_advert(advert, sizeof(advert));

    TEST_ASSERT_FALSE(r.hit);
}

static void test_parse_advert_truncated_length_safe() {
    // field_len claims more bytes than the buffer actually has — must not
    // read out of bounds or crash, just fail to find a hit.
    uint8_t advert[4] = { 0xFF, AD_TYPE_SVC_DATA_UUID16, 0xFA, 0xFF };
    OdidParseResult r = odid_parse_advert(advert, sizeof(advert));

    TEST_ASSERT_FALSE(r.hit);
}

static void test_parse_advert_empty_buffer_safe() {
    OdidParseResult r = odid_parse_advert(nullptr, 0);
    TEST_ASSERT_FALSE(r.hit);
}

// ---------------------------------------------------------------------------
// Unity entrypoints
// ---------------------------------------------------------------------------

void setUp() {}
void tearDown() {}

int main(int argc, char** argv) {
    UNITY_BEGIN();
    RUN_TEST(test_location_roundtrip_known_vector);
    RUN_TEST(test_location_rejects_wrong_message_type);
    RUN_TEST(test_location_rejects_short_buffer);
    RUN_TEST(test_basic_id_serial_extracted_and_trimmed);
    RUN_TEST(test_basic_id_rejects_wrong_message_type);
    RUN_TEST(test_parse_advert_svc_data16_basic_id);
    RUN_TEST(test_parse_advert_svc_data16_location);
    RUN_TEST(test_parse_advert_dji_manufacturer_data);
    RUN_TEST(test_parse_advert_non_odid_ignored);
    RUN_TEST(test_parse_advert_truncated_length_safe);
    RUN_TEST(test_parse_advert_empty_buffer_safe);
    return UNITY_END();
}
