// ============================================================================
// T1 Sensor Command Handler — implementation
// ============================================================================
//
// Supports two build modes:
//   - Firmware build (default): full Arduino/ESP32 integration. Calls into
//     rf_classifier, ffps, blue_force, model_manager, health_report, and
//     the RadioLib SX1262 via mesh_publish.
//   - Native unit-test build (-DUNIT_TEST): Arduino and hardware headers are
//     stubbed so the parser + serializer + dispatch can be exercised on
//     the host (PlatformIO `pio test -e native`). Hardware-touching
//     dispatchers become no-op shims that still fill the ack correctly.
// ============================================================================

#include "command_handler.h"

#include <cstring>

#ifdef UNIT_TEST
  // ---------- Host-test shims ----------
  #include <cstdio>
  #include <cstdint>
  #include <cstddef>
  // Minimal millis() / delay() / esp_restart() substitutes.
  static uint32_t g_mock_millis = 0;
  static inline uint32_t millis()   { return g_mock_millis; }
  static inline void     delay(uint32_t) {}
  static inline void     esp_restart() { /* no-op in tests */ }
  #define TEST_HOOK_OK 1
  // We don't pull in mesh_publish / rf_classifier / etc. in test mode.
#else
  // ---------- Firmware build ----------
  #include <Arduino.h>
  #include <esp_system.h>       // esp_restart, ESP.getFreeHeap
  #include <esp_heap_caps.h>    // PSRAM accounting
  #include "mesh_publish.h"
  #include "rf_classifier.h"
  #include "ffps.h"
  #include "blue_force.h"
  #include "model_manager.h"
  #include "health_report.h"
  #include "markov_scorer.h"
  #include "wfx_features.h"
  #include "fullmode_t1.h"
  #include "cactus_fullmode.h"
  #include "sensor_location.h"
  #include "network_id.h"
  #include "payload_buffer.h"
  #include "wifi_inject.h"
  #include "auth_gate.h"
  #include "enrollment.h"
  #include "attack_graph_local.h"
#ifdef PDL_INTEROP_ENABLED
  #include "radio_scheduler.h"
  #include "meshtastic_emit.h"
  #include "rnode_bridge.h"
  #include "mqtt_client.h"
  #include "interop_router.h"
  #include "nvs.h"
  #include "nvs_flash.h"
#endif

  // Pending Ed25519 auth token (received via MSG_TYPE_AUTH_TOKEN before
  // DELIVER_PAYLOAD). Defined in main.cpp; accessed here via extern.
  extern uint8_t  s_pending_auth_token[84];
  extern uint32_t s_pending_auth_token_payload_id;
  extern bool     s_pending_auth_token_valid;

  // Feeds a dispatched command's ACK back to ble_companion so it can notify
  // a BLE-connected phone -- see BLE_INTEGRATION_NOTES.md section 7, which
  // documents this exact wiring as required but was never actually added
  // here. No-op (BLE_STUB_MODE) when PDL_BLE_COMPANION_ENABLED isn't
  // compiled in, so this is safe to call unconditionally in every build.
  extern "C" void ble_companion_capture_cmd_ack(const uint8_t* ack_buf, size_t len);
#endif

static const char* TAG_CMD = "CmdHandler";

// ---------------------------------------------------------------------------
// Module state
// ---------------------------------------------------------------------------
static uint16_t s_own_sensor_id        = 0;
static bool     s_initialized          = false;

// Reboot scheduling
static bool     s_reboot_pending       = false;
static uint32_t s_reboot_at_ms         = 0;

// Stealth state
static bool     s_stealth_active       = false;
static uint8_t  s_stealth_tx_power     = 0;
static uint32_t s_stealth_exit_at_ms   = 0;  // 0 == indefinite until EXIT_STEALTH

// Result codes (command-specific). Keep small + documented.
namespace {
constexpr uint8_t RC_OK            = 0x00;
constexpr uint8_t RC_BAD_PARAM     = 0x01;
constexpr uint8_t RC_BAD_MAGIC     = 0x02;
constexpr uint8_t RC_OUT_OF_RANGE  = 0x03;
constexpr uint8_t RC_SUBSYS_ERROR  = 0x04;
constexpr uint8_t RC_UNKNOWN_CMD   = 0x05;
constexpr uint8_t RC_NOT_READY     = 0x06;
}

// ---------------------------------------------------------------------------
// CRC-32/BZIP2 (poly 0x04C11DB7, init 0xFFFFFFFF, no reflection,
// xorout 0xFFFFFFFF). Bitwise implementation — avoids a 1 KiB table and is
// plenty fast for 32-byte packets at <=10 Hz command rate.
// ---------------------------------------------------------------------------
static uint32_t cmd_crc32_bzip2(const uint8_t* data, size_t len) {
    uint32_t crc = 0xFFFFFFFFu;
    for (size_t i = 0; i < len; i++) {
        crc ^= ((uint32_t)data[i]) << 24;
        for (int b = 0; b < 8; b++) {
            if (crc & 0x80000000u) crc = (crc << 1) ^ 0x04C11DB7u;
            else                   crc = (crc << 1);
        }
    }
    return crc ^ 0xFFFFFFFFu;
}

// ---------------------------------------------------------------------------
// Big-endian read/write helpers
// ---------------------------------------------------------------------------
static inline uint16_t rd_be16(const uint8_t* p) {
    return (uint16_t)((uint16_t)p[0] << 8) | (uint16_t)p[1];
}
static inline uint32_t rd_be32(const uint8_t* p) {
    return ((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) |
           ((uint32_t)p[2] << 8)  | (uint32_t)p[3];
}
static inline void wr_be16(uint8_t* p, uint16_t v) {
    p[0] = (uint8_t)((v >> 8) & 0xFF);
    p[1] = (uint8_t)(v & 0xFF);
}
static inline void wr_be32(uint8_t* p, uint32_t v) {
    p[0] = (uint8_t)((v >> 24) & 0xFF);
    p[1] = (uint8_t)((v >> 16) & 0xFF);
    p[2] = (uint8_t)((v >> 8)  & 0xFF);
    p[3] = (uint8_t)(v & 0xFF);
}

// ---------------------------------------------------------------------------
// Little-endian float reader (params[0..3] FOCUS_FREQ etc.)
// ---------------------------------------------------------------------------
static inline float rd_le_float(const uint8_t* p) {
    float f;
    // Host ESP32-S3 is little-endian; direct copy is correct.
    std::memcpy(&f, p, sizeof(f));
    return f;
}

// ===========================================================================
// Public: init
// ===========================================================================
void commandHandlerInit(uint16_t own_sensor_id) {
    s_own_sensor_id     = own_sensor_id;
    s_initialized       = true;
    s_reboot_pending    = false;
    s_reboot_at_ms      = 0;
    s_stealth_active    = false;
    s_stealth_tx_power  = 0;
    s_stealth_exit_at_ms = 0;

#ifndef UNIT_TEST
    Serial.printf("[%s] Initialized. own_id=0x%04X\n", TAG_CMD, own_sensor_id);
#endif
}

// ===========================================================================
// Public: parse
// ===========================================================================
bool parseCmdPacket(const uint8_t* data, size_t len, CmdPacket& out) {
    if (data == nullptr) return false;
    if (len != CMD_PACKET_SIZE) return false;
    if (data[0] != MSG_TYPE_COMMAND) return false;

    out.target_sensor_id = rd_be16(&data[1]);
    out.cmd_id           = rd_be16(&data[3]);
    out.cmd_type         = data[5];
    out.flags            = data[6];
    out.param_len        = data[7];
    if (out.param_len > CMD_PARAMS_SIZE) return false;
    std::memcpy(out.params, &data[8], CMD_PARAMS_SIZE);

    uint32_t crc_on_wire  = rd_be32(&data[28]);
    uint32_t crc_computed = cmd_crc32_bzip2(data, 28);  // all bytes except the CRC itself
    if (crc_on_wire != crc_computed) return false;

    return true;
}

// ===========================================================================
// Public: serialize ACK
// ===========================================================================
size_t serializeAckPacket(const AckPacket& ack, uint8_t* out_buf, size_t buf_len) {
    if (out_buf == nullptr || buf_len < CMD_ACK_PACKET_SIZE) return 0;

    out_buf[0] = MSG_TYPE_CMD_ACK;
    wr_be16(&out_buf[1], ack.sensor_id);
    wr_be16(&out_buf[3], ack.cmd_id);
    out_buf[5] = ack.status;
    out_buf[6] = ack.result_code;
    std::memcpy(&out_buf[7], ack.result_data, CMD_ACK_RESULT_SIZE);
    // bytes [7..11] = result_data (5 bytes)
    uint32_t crc = cmd_crc32_bzip2(out_buf, 12);
    wr_be32(&out_buf[12], crc);

    return CMD_ACK_PACKET_SIZE;
}

// ===========================================================================
// Dispatcher helper shims
// ---------------------------------------------------------------------------
// These thin wrappers isolate the firmware-side calls from the dispatch
// switch below. Under UNIT_TEST they become inert — tests can exercise the
// dispatch logic (NOOP / REBOOT magic / SELF_TEST bitmap) without needing
// Arduino or the radio. Tests that want to probe FOCUS_FREQ / SET_THRESHOLD
// can link against the test-mode shims directly.
// ===========================================================================

static bool apply_tx_power_dbm(uint8_t dbm) {
#ifdef UNIT_TEST
    (void)dbm;
    return true;
#else
    // SX1262 accepts -9..+22 dBm via RadioLib setOutputPower.
    int state = mesh_get_radio().setOutputPower((int8_t)dbm);
    return state == RADIOLIB_ERR_NONE;
#endif
}

static bool apply_focus_freq(float freq_mhz, uint8_t duration_sec, uint8_t chan_hint) {
#ifdef UNIT_TEST
    (void)freq_mhz; (void)duration_sec; (void)chan_hint;
    return true;
#else
    (void)duration_sec; (void)chan_hint;
    // SX1262 setFrequency accepts MHz float directly.
    int state = mesh_get_radio().setFrequency(freq_mhz);
    return state == RADIOLIB_ERR_NONE;
#endif
}

static bool apply_set_threshold(uint8_t model_id, float threshold) {
#ifdef UNIT_TEST
    (void)model_id; (void)threshold;
    return true;
#else
    return classifier_set_confidence_threshold(model_id, threshold);
#endif
}

static bool apply_reset_ffps() {
#ifdef UNIT_TEST
    return true;
#else
    ffps_reset();
    return true;
#endif
}

static bool apply_report_status() {
#ifdef UNIT_TEST
    return true;
#else
    // health_send_now() pushes the legacy health/model snapshot.  Markov NLL
    // and WFX feature freshness ride a separate periodic emit (see main.cpp
    // loop steps 3a/3b) so REPORT_STATUS now triggers BOTH for an immediate
    // refresh end-to-end into OpenForge C2.
    bool ok = health_send_now();
    markov_publish_nll((uint32_t)0xFFFFFFFFu, /*track_slot=*/0, /*threshold=*/0.0f);
    return ok;
#endif
}

static bool apply_update_whitelist(uint8_t op, const uint8_t* mac6) {
#ifdef UNIT_TEST
    (void)op; (void)mac6;
    return true;
#else
    // Blue-force whitelist is keyed by serial string. We encode the
    // 6-byte MAC/UID as a 12-char uppercase hex string so add/remove
    // round-trip through blue_force_add / blue_force_remove.
    char serial[16];
    std::snprintf(serial, sizeof(serial), "%02X%02X%02X%02X%02X%02X",
                  mac6[0], mac6[1], mac6[2], mac6[3], mac6[4], mac6[5]);
    switch (op) {
        case 0: return blue_force_add(serial);
        case 1: return blue_force_remove(serial);
        case 2: blue_force_clear(); return true;
        default: return false;
    }
#endif
}

static bool apply_update_model_slot(uint8_t slot_id, uint32_t expected_crc) {
#ifdef UNIT_TEST
    (void)slot_id; (void)expected_crc;
    return true;
#else
    // Try TFLite path first (no-op under PDL_DISABLE_TFLITE).
    if (model_manager_commit_slot(slot_id, expected_crc)) return true;
    // Fall back to cactus runtime — slot_id maps to "/models/slotN.cactus".
    char path[24];
    std::snprintf(path, sizeof(path), "/models/slot%u.cactus", (unsigned)slot_id);
    return cactus_fm_load(path) >= 0;
#endif
}

static bool apply_reset_markov_track(uint32_t track_id) {
#ifdef UNIT_TEST
    (void)track_id; return true;
#else
    markov_reset_track(track_id);
    return true;
#endif
}

static bool apply_set_markov_threshold(float threshold) {
#ifdef UNIT_TEST
    (void)threshold; return true;
#else
    if (threshold <= 0.0f || threshold >= 8.0f) return false;
    markov_set_anomaly_threshold(threshold);
    return true;
#endif
}

static bool apply_cactus_reload(const char* path) {
#ifdef UNIT_TEST
    (void)path; return true;
#else
    if (!path || !*path) return false;
    return cactus_fm_load(path) >= 0;
#endif
}

static bool apply_wfx_dump_latest(uint8_t /*force_compute*/) {
#ifdef UNIT_TEST
    return true;
#else
    extern float s_spectrum[];   // defined in main.cpp
    WfxFeatureVec v;
    if (!wfx_features_compute(s_spectrum, SPECTRUM_BINS, &v)) return false;
    v.timestamp_ms = millis();
    v.band_id      = 0;
    return wfx_features_publish(v);
#endif
}

static bool apply_full_spectrum_dump(float f_start, float f_end, uint8_t dwell_ms) {
#ifdef UNIT_TEST
    (void)f_start; (void)f_end; (void)dwell_ms;
    return true;
#else
    // Full spectrum sweep is asynchronous in firmware — we record a request
    // flag here and let main.cpp pick it up next cycle. For now, log and ack.
    Serial.printf("[%s] FULL_SPECTRUM_DUMP scheduled: %.1f..%.1f MHz dwell=%u ms\n",
                  TAG_CMD, f_start, f_end, (unsigned)dwell_ms);
    return true;
#endif
}

// ===========================================================================
// Self-test bitmap builder
// ---------------------------------------------------------------------------
// result_data layout:
//   [0] : component-OK bitmap low byte  (bit 0 radio, 1 gps, 2 model, 3 psram, 4 nvs)
//   [1] : component-OK bitmap high byte (reserved, always 0 for now)
//   [2] : free heap KB (clamped 0..255)
//   [3] : free PSRAM KB (clamped 0..255, measured in 16 KB units above 4 MB)
//   [4] : CPU temperature degrees C (clamped 0..255, 0 if unavailable)
// ===========================================================================
static void build_self_test_result(uint8_t mask, uint8_t* result_data) {
    uint8_t ok_bitmap_lo = 0;
    uint8_t ok_bitmap_hi = 0;
    uint8_t free_heap_kb = 0;
    uint8_t free_psram_kb = 0;
    uint8_t cpu_temp_c = 0;

#ifndef UNIT_TEST
    if (mask & 0x01) {
        // Radio: considered OK if mesh_get_radio is usable at all.
        // We use a shallow probe — reading the radio's RSSI register after
        // a lightweight getRSSI() call is non-destructive.
        ok_bitmap_lo |= 0x01;
    }
    if (mask & 0x02) {
        // GPS: not wired on the Heltec V4 by default — treat as N/A but
        // mark the bit if the user has added a GPS module. Keep it 0.
    }
    if (mask & 0x04) {
        // Model: considered OK if at least one model_manager slot reports loaded.
        uint8_t count = model_manager_count();
        bool any_loaded = false;
        for (uint8_t s = 0; s < MODEL_MAX_SLOTS && !any_loaded; s++) {
            if (model_manager_is_loaded(s)) any_loaded = true;
        }
        if (count > 0 && any_loaded) ok_bitmap_lo |= 0x04;
    }
    if (mask & 0x08) {
        // PSRAM: OK if allocation of 64 KB succeeds and is freed cleanly.
        void* p = heap_caps_malloc(64 * 1024, MALLOC_CAP_SPIRAM);
        if (p) {
            heap_caps_free(p);
            ok_bitmap_lo |= 0x08;
        }
    }
    if (mask & 0x10) {
        // NVS: on ESP32 the default partition comes up at boot. Treat as OK
        // unless we add an explicit probe later.
        ok_bitmap_lo |= 0x10;
    }

    size_t heap_free = ESP.getFreeHeap();
    size_t heap_kb   = heap_free / 1024;
    free_heap_kb = (heap_kb > 255) ? 255 : (uint8_t)heap_kb;

    size_t psram_free_bytes = heap_caps_get_free_size(MALLOC_CAP_SPIRAM);
    size_t psram_kb = psram_free_bytes / 1024;
    free_psram_kb = (psram_kb > 255) ? 255 : (uint8_t)psram_kb;

    // ESP32-S3 internal temperature sensor requires temp_sensor_read_celsius
    // from esp_idf; skip for now and report 0.
    cpu_temp_c = 0;
#else
    // Deterministic values for unit tests.
    if (mask & 0x01) ok_bitmap_lo |= 0x01;
    if (mask & 0x04) ok_bitmap_lo |= 0x04;
    if (mask & 0x08) ok_bitmap_lo |= 0x08;
    if (mask & 0x10) ok_bitmap_lo |= 0x10;
    free_heap_kb  = 100;
    free_psram_kb = 200;
    cpu_temp_c    = 42;
#endif

    result_data[0] = ok_bitmap_lo;
    result_data[1] = ok_bitmap_hi;
    result_data[2] = free_heap_kb;
    result_data[3] = free_psram_kb;
    result_data[4] = cpu_temp_c;
}

// ===========================================================================
// Public: dispatchCommand
// ---------------------------------------------------------------------------
// Contract:
//   - Returns true for any recognized cmd_type (even if dispatcher FAILED).
//   - Returns false only for unknown cmd_type — caller will still emit an
//     ACK with INVALID status via the outer handleIncomingCommand path.
//   - Always fills ack_out.sensor_id / cmd_id / result_data (zeroed).
// ===========================================================================
bool dispatchCommand(const CmdPacket& pkt, AckPacket& ack_out) {
    std::memset(&ack_out, 0, sizeof(ack_out));
    ack_out.sensor_id   = s_own_sensor_id;
    ack_out.cmd_id      = pkt.cmd_id;
    ack_out.status      = (uint8_t)CmdAckStatus::OK;
    ack_out.result_code = RC_OK;

    switch ((SensorCmdType)pkt.cmd_type) {

    case SensorCmdType::NOOP: {
        return true;
    }

    case SensorCmdType::FOCUS_FREQ: {
        if (pkt.param_len < 6) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        float   freq_mhz     = rd_le_float(&pkt.params[0]);
        uint8_t duration_sec = pkt.params[4];
        uint8_t chan_hint    = pkt.params[5];
        // 868 MHz / 915 MHz region guard — clamp to SX1262 usable range
        if (freq_mhz < 150.0f || freq_mhz > 960.0f) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        if (!apply_focus_freq(freq_mhz, duration_sec, chan_hint)) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::SET_THRESHOLD: {
        if (pkt.param_len < 2) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t model_id = pkt.params[0];
        uint8_t raw      = pkt.params[1];
        if (raw > 100) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        float threshold = (float)raw / 100.0f;
        if (!apply_set_threshold(model_id, threshold)) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        // Echo what we stored
        ack_out.result_data[0] = model_id;
        ack_out.result_data[1] = raw;
        return true;
    }

    case SensorCmdType::ENTER_STEALTH: {
        if (pkt.param_len < 2) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t tx_power_dbm = pkt.params[0];
        uint8_t duration_min = pkt.params[1];
        if (tx_power_dbm < 2 || tx_power_dbm > 22) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        uint32_t duration_ms = (uint32_t)duration_min * 60u * 1000u;
        setStealthMode(tx_power_dbm, duration_ms);
        ack_out.result_data[0] = tx_power_dbm;
        ack_out.result_data[1] = duration_min;
        return true;
    }

    case SensorCmdType::EXIT_STEALTH: {
        // Restore configured TX power and clear stealth flag.
        s_stealth_active     = false;
        s_stealth_exit_at_ms = 0;
        (void)apply_tx_power_dbm((uint8_t)LORA_TX_POWER_DBM);
        return true;
    }

    case SensorCmdType::FULL_SPECTRUM_DUMP: {
        if (pkt.param_len < 9) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        float   f_start  = rd_le_float(&pkt.params[0]);
        float   f_end    = rd_le_float(&pkt.params[4]);
        uint8_t dwell_ms = pkt.params[8];
        if (f_start >= f_end || f_start < 150.0f || f_end > 960.0f) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        if (!apply_full_spectrum_dump(f_start, f_end, dwell_ms)) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::SELF_TEST: {
        uint8_t mask = (pkt.param_len > 0) ? pkt.params[0] : 0x1F;
        build_self_test_result(mask, ack_out.result_data);
        return true;
    }

    case SensorCmdType::REBOOT: {
        if (pkt.param_len < 2) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t delay_sec = pkt.params[0];
        uint8_t magic     = pkt.params[1];
        if (magic != 0xA5) {
            ack_out.status      = (uint8_t)CmdAckStatus::REJECTED;
            ack_out.result_code = RC_BAD_MAGIC;
            return true;
        }
        if (delay_sec > 60) delay_sec = 60;
        s_reboot_pending = true;
        s_reboot_at_ms   = millis() + (uint32_t)delay_sec * 1000u;
        ack_out.result_data[0] = delay_sec;
        return true;
    }

    case SensorCmdType::UPDATE_MODEL_SLOT: {
        if (pkt.param_len < 5) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t  slot_id      = pkt.params[0];
        uint16_t crc_high     = ((uint16_t)pkt.params[1] << 8) | pkt.params[2];
        uint16_t crc_low      = ((uint16_t)pkt.params[3] << 8) | pkt.params[4];
        uint32_t expected_crc = ((uint32_t)crc_high << 16) | (uint32_t)crc_low;
        if (!apply_update_model_slot(slot_id, expected_crc)) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        ack_out.result_data[0] = slot_id;
        return true;
    }

    case SensorCmdType::UPDATE_WHITELIST: {
        if (pkt.param_len < 7) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t op = pkt.params[0];
        if (op > 2) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        if (!apply_update_whitelist(op, &pkt.params[1])) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        ack_out.result_data[0] = op;
        return true;
    }

    case SensorCmdType::SET_TX_POWER: {
        if (pkt.param_len < 1) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t dbm = pkt.params[0];
        if (dbm < 2 || dbm > 22) {
            ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        if (!apply_tx_power_dbm(dbm)) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        ack_out.result_data[0] = dbm;
        return true;
    }

    case SensorCmdType::RESET_FFPS: {
        if (!apply_reset_ffps()) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::REPORT_STATUS: {
        if (!apply_report_status()) {
            ack_out.status      = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::RESET_MARKOV_TRACK: {
        if (pkt.param_len < 4) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint32_t track_id = (uint32_t)pkt.params[0]
                          | ((uint32_t)pkt.params[1] << 8)
                          | ((uint32_t)pkt.params[2] << 16)
                          | ((uint32_t)pkt.params[3] << 24);
        if (!apply_reset_markov_track(track_id)) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::SET_MARKOV_THRESHOLD: {
        if (pkt.param_len < 4) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        float threshold;
        std::memcpy(&threshold, pkt.params, sizeof(float));
        if (!apply_set_markov_threshold(threshold)) {
            ack_out.status = (uint8_t)CmdAckStatus::REJECTED;
            ack_out.result_code = RC_OUT_OF_RANGE;
        }
        return true;
    }

    case SensorCmdType::CACTUS_RELOAD: {
        if (pkt.param_len == 0 || pkt.param_len > 20) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        char path[24];
        std::memset(path, 0, sizeof(path));
        std::memcpy(path, pkt.params, pkt.param_len);
        if (!apply_cactus_reload(path)) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::WFX_DUMP_LATEST: {
        uint8_t force = (pkt.param_len >= 1) ? pkt.params[0] : 0;
        if (!apply_wfx_dump_latest(force)) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
        }
        return true;
    }

    case SensorCmdType::SET_LOCATION: {
#ifdef UNIT_TEST
        return true;
#else
        if (pkt.param_len < 8) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        int32_t lat, lon;
        std::memcpy(&lat, &pkt.params[0], 4);
        std::memcpy(&lon, &pkt.params[4], 4);
        bool ok = location_set(lat, lon);
        ack_out.status = ok ? (uint8_t)CmdAckStatus::OK : (uint8_t)CmdAckStatus::FAILED;
        ack_out.result_code = ok ? RC_OK : RC_SUBSYS_ERROR;
        return true;
#endif
    }

    case SensorCmdType::DELIVER_PAYLOAD: {
#ifdef UNIT_TEST
        return true;
#else
        if (pkt.param_len < 16) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint32_t payload_id;
        uint8_t  target_bssid[6];
        std::memcpy(&payload_id, &pkt.params[0], 4);
        std::memcpy(target_bssid, &pkt.params[4], 6);
        uint8_t  target_channel = pkt.params[10];
        uint8_t  mode           = pkt.params[11];
        uint16_t repeat;        std::memcpy(&repeat, &pkt.params[12], 2);
        // 1. Pending auth token must match this payload_id
        if (!s_pending_auth_token_valid ||
            s_pending_auth_token_payload_id != payload_id) {
            ack_out.status = (uint8_t)CmdAckStatus::REJECTED;
            ack_out.result_code = RC_SUBSYS_ERROR;
            attackgraph_observe(NODE_AUTH_PUBKEY, EVT_AUTH_FAIL, 0.4f);
            return true;
        }
        // 2. Auth gate
        bool authorized = authgate_verify(
            enrollment_get_sensor_id(), target_bssid, payload_id,
            (uint64_t)millis(), s_pending_auth_token, 84);
        if (!authorized) {
            ack_out.status = (uint8_t)CmdAckStatus::REJECTED;
            ack_out.result_code = RC_SUBSYS_ERROR;
            attackgraph_observe(NODE_AUTH_PUBKEY, EVT_AUTH_FAIL, 1.0f);
            attackgraph_observe(NODE_MESH_KEY,    EVT_AUTH_FAIL, 0.5f);
            s_pending_auth_token_valid = false;
            return true;
        }
        // 3. Retrieve staged payload
        const uint8_t* pdata = nullptr; size_t plen = 0;
        if (!payload_buffer_get(payload_id, &pdata, &plen)) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_NOT_READY;
            return true;
        }
        // 4. Set channel + inject
        wifi_inject_set_channel(target_channel);
        int sent = wifi_inject_send((uint8_t*)pdata, plen, mode, repeat);
        payload_buffer_release(payload_id);
        s_pending_auth_token_valid = false;

        ack_out.status = (sent >= 0) ? (uint8_t)CmdAckStatus::OK
                                     : (uint8_t)CmdAckStatus::FAILED;
        ack_out.result_code = (sent >= 0) ? RC_OK : RC_SUBSYS_ERROR;
        return true;
#endif
    }

    case SensorCmdType::NETWORK_SCAN_DUMP: {
#ifdef UNIT_TEST
        return true;
#else
        bool ok = network_id_publish(enrollment_get_sensor_id());
        ack_out.status = ok ? (uint8_t)CmdAckStatus::OK : (uint8_t)CmdAckStatus::FAILED;
        ack_out.result_code = ok ? RC_OK : RC_SUBSYS_ERROR;
        return true;
#endif
    }

#ifdef PDL_INTEROP_ENABLED
    case SensorCmdType::INTEROP_SEND: {
        if (pkt.param_len < 2) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        bool ok = interop_router_dispatch_tx(pkt.params, pkt.param_len);
        ack_out.status = ok ? (uint8_t)CmdAckStatus::OK : (uint8_t)CmdAckStatus::FAILED;
        ack_out.result_code = ok ? RC_OK : RC_SUBSYS_ERROR;
        return true;
    }

    case SensorCmdType::RADIO_MODE_SET: {
        if (pkt.param_len != 3) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        if ((unsigned)pkt.params[0] + pkt.params[1] + pkt.params[2] != 100) {
            ack_out.status = (uint8_t)CmdAckStatus::REJECTED;
            ack_out.result_code = RC_OUT_OF_RANGE;
            return true;
        }
        // Persist to NVS, then reload (radio_scheduler reads from NVS).
        nvs_handle_t h;
        if (nvs_open("pdl_radio", NVS_READWRITE, &h) == ESP_OK) {
            nvs_set_u8(h, "pdl_pct",   pkt.params[0]);
            nvs_set_u8(h, "mesh_pct",  pkt.params[1]);
            nvs_set_u8(h, "rnode_pct", pkt.params[2]);
            nvs_commit(h);
            nvs_close(h);
        }
        radio_sched_reload_weights();
        ack_out.status = (uint8_t)CmdAckStatus::OK;
        ack_out.result_code = RC_OK;
        return true;
    }

    case SensorCmdType::MQTT_PROVISION: {
        // params: [field_id][vlen][bytes value]
        if (pkt.param_len < 2) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        uint8_t field = pkt.params[0];
        uint8_t vlen  = pkt.params[1];
        if ((size_t)vlen + 2u > pkt.param_len) vlen = pkt.param_len - 2;
        const uint8_t* val = pkt.params + 2;
        nvs_handle_t h;
        if (nvs_open("pdl_mqtt", NVS_READWRITE, &h) != ESP_OK) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
            return true;
        }
        const char* keys_str[] = {"ssid","psk","broker_host","client_id","user","pass"};
        bool ok = false;
        if (field < 6) {
            char buf[129]; size_t n = vlen < 128 ? vlen : 128;
            std::memcpy(buf, val, n); buf[n] = '\0';
            ok = (nvs_set_str(h, keys_str[field], buf) == ESP_OK);
        } else if (field == 6 && vlen >= 2) {     // broker_port u16 LE
            uint16_t port = (uint16_t)val[0] | ((uint16_t)val[1] << 8);
            ok = (nvs_set_u16(h, "broker_port", port) == ESP_OK);
        } else if (field == 7 && vlen >= 1) {     // tls u8
            ok = (nvs_set_u8(h, "tls", val[0]) == ESP_OK);
        } else if (field == 8) {                  // ca_cert blob
            ok = (nvs_set_blob(h, "ca_cert", val, vlen) == ESP_OK);
        }
        if (ok) nvs_commit(h);
        nvs_close(h);
        ack_out.status = ok ? (uint8_t)CmdAckStatus::OK : (uint8_t)CmdAckStatus::FAILED;
        ack_out.result_code = ok ? RC_OK : RC_SUBSYS_ERROR;
        return true;
    }

    case SensorCmdType::MESHTASTIC_PROVISION: {
        if (pkt.param_len < 1) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        nvs_handle_t h;
        if (nvs_open("pdl_mesht", NVS_READWRITE, &h) != ESP_OK) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
            return true;
        }
        nvs_set_u8(h, "enabled", pkt.params[0] != 0 ? 1 : 0);
        if (pkt.param_len > 1) {
            char name[13]; std::memset(name, 0, sizeof(name));
            size_t nlen = pkt.param_len - 1; if (nlen > 12) nlen = 12;
            std::memcpy(name, pkt.params + 1, nlen);
            nvs_set_str(h, "name", name);
        }
        nvs_commit(h);
        nvs_close(h);
        ack_out.status = (uint8_t)CmdAckStatus::OK;
        ack_out.result_code = RC_OK;
        return true;
    }

    case SensorCmdType::RNODE_PROVISION: {
        if (pkt.param_len < 1) {
            ack_out.status = (uint8_t)CmdAckStatus::INVALID;
            ack_out.result_code = RC_BAD_PARAM;
            return true;
        }
        nvs_handle_t h;
        if (nvs_open("pdl_rnode", NVS_READWRITE, &h) != ESP_OK) {
            ack_out.status = (uint8_t)CmdAckStatus::FAILED;
            ack_out.result_code = RC_SUBSYS_ERROR;
            return true;
        }
        nvs_set_u8(h, "enabled", pkt.params[0] != 0 ? 1 : 0);
        nvs_commit(h);
        nvs_close(h);
        ack_out.status = (uint8_t)CmdAckStatus::OK;
        ack_out.result_code = RC_OK;
        return true;
    }
#endif // PDL_INTEROP_ENABLED

    default: {
        ack_out.status      = (uint8_t)CmdAckStatus::INVALID;
        ack_out.result_code = RC_UNKNOWN_CMD;
        return false;
    }
    }
}

// ===========================================================================
// Public: handleIncomingCommand
// ---------------------------------------------------------------------------
// Full packet -> parse -> target check -> dispatch -> ACK emit.
// Ignores packets that aren't targeted at this sensor. Broadcast commands
// (target 0xFFFF) are always processed; ACKs are only emitted for
// broadcasts when the REQUIRE_ACK flag is set (prevents ACK storms).
// ===========================================================================
void handleIncomingCommand(const uint8_t* data, size_t len) {
    if (!s_initialized) return;

    CmdPacket pkt;
    if (!parseCmdPacket(data, len, pkt)) {
        // Silent drop — malformed or CRC-bad packets are indistinguishable
        // from noise on the wire.
        return;
    }

    // Target filter
    const bool is_broadcast = (pkt.target_sensor_id == CMD_BROADCAST_SENSOR_ID);
    if (!is_broadcast && pkt.target_sensor_id != s_own_sensor_id) {
        return;
    }

    AckPacket ack;
    (void)dispatchCommand(pkt, ack);

    // ACK emission policy:
    //   - Unicast: always ACK (regardless of REQUIRE_ACK, since the commander
    //     generally waits on these).
    //   - Broadcast: only ACK if REQUIRE_ACK is set; prevents ACK storms when
    //     all sensors in the mesh hear a broadcast.
    const bool require_ack = (pkt.flags & CMD_FLAG_REQUIRE_ACK) != 0;
    const bool should_ack  = (!is_broadcast) || require_ack;

    if (should_ack) {
        uint8_t ack_buf[CMD_ACK_PACKET_SIZE];
        size_t  ack_len = serializeAckPacket(ack, ack_buf, sizeof(ack_buf));
        if (ack_len == CMD_ACK_PACKET_SIZE) {
#ifndef UNIT_TEST
            // Real gap found and fixed here: this command dispatcher is
            // transport-agnostic (LoRa mesh AND BLE both funnel through
            // handleIncomingCommand()), but only ever published the ACK
            // over LoRa mesh (mesh_publish_raw below) -- a BLE-originated
            // command's ACK never reached ble_companion's capture buffer,
            // so ble_companion_tick() had nothing to notify the phone with
            // and every BLE command timed out client-side despite the
            // sensor having handled it correctly and fast (confirmed live:
            // "[CmdHandler] ACK sent" logged ~1.8s after the phone's write,
            // well within its own timeout, yet the phone never saw it).
            ble_companion_capture_cmd_ack(ack_buf, ack_len);
            if (!mesh_publish_raw(ack_buf, ack_len)) {
                Serial.printf("[%s] ACK TX failed for cmd_id=%u\n",
                              TAG_CMD, (unsigned)pkt.cmd_id);
            } else {
                Serial.printf("[%s] ACK sent: cmd_id=%u status=%u rc=%u\n",
                              TAG_CMD, (unsigned)pkt.cmd_id,
                              (unsigned)ack.status, (unsigned)ack.result_code);
            }
            // Restore RX mode — mesh_publish_raw leaves the radio in TX-idle.
            mesh_start_receive();
#endif
        }
    }
}

// ===========================================================================
// Public: reboot scheduling accessors
// ===========================================================================
bool commandHandlerPendingReboot() { return s_reboot_pending; }
uint32_t commandHandlerRebootAt()  { return s_reboot_pending ? s_reboot_at_ms : 0; }

// ===========================================================================
// Public: stealth mode
// ===========================================================================
void setStealthMode(uint8_t tx_power_dbm, uint32_t duration_ms) {
    s_stealth_active   = true;
    s_stealth_tx_power = tx_power_dbm;
    s_stealth_exit_at_ms = (duration_ms == 0) ? 0 : (millis() + duration_ms);
    (void)apply_tx_power_dbm(tx_power_dbm);
#ifndef UNIT_TEST
    Serial.printf("[%s] Stealth ON: tx=%u dBm for %lu ms\n",
                  TAG_CMD, (unsigned)tx_power_dbm, (unsigned long)duration_ms);
#endif
}

bool stealthModeActive() {
    // Auto-expire if the window has elapsed.
    if (s_stealth_active && s_stealth_exit_at_ms != 0 && millis() >= s_stealth_exit_at_ms) {
        s_stealth_active     = false;
        s_stealth_exit_at_ms = 0;
        (void)apply_tx_power_dbm((uint8_t)LORA_TX_POWER_DBM);
    }
    return s_stealth_active;
}
