/**
 * @file NimBLEPlatform.cpp
 * @brief NimBLE-Arduino implementation for ESP32
 */

#include "NimBLEPlatform.h"

#if defined(ESP32) && (defined(USE_NIMBLE) || defined(CONFIG_BT_NIMBLE_ENABLED))

#include "Log.h"
#include "Identity.h"
#include <algorithm>
#include <esp_mac.h>

// WiFi coexistence: Check if WiFi is available and connected
// This is used to add extra delays before BLE connection attempts
#if __has_include(<WiFi.h>)
    #include <WiFi.h>
    #define HAS_WIFI_COEX 1
#else
    #define HAS_WIFI_COEX 0
#endif

// NimBLE low-level GAP functions for checking stack state and native connections
extern "C" {
    #include "nimble/nimble/host/include/host/ble_gap.h"
    #include "nimble/nimble/host/include/host/ble_hs.h"

    int ble_gap_adv_active(void);
    int ble_gap_disc_active(void);
    int ble_gap_conn_active(void);

    // Host reset — enqueues a reset event that clears GAP state and resyncs
    // with the BLE controller without touching heap-corrupting deinit paths.
    void ble_hs_sched_reset(int reason);
}

// Defined in patched NimBLEDevice.cpp — set in onReset callback with the reason code.
// Poll from BLE loop to log via UDP (NimBLE's own logging only reaches serial UART).
extern volatile int nimble_host_reset_reason;

namespace RNS { namespace BLE {

//=============================================================================
// Static Member Initialization
//=============================================================================

// Unclean shutdown flag - persists across soft reboot on ESP32
// RTC_NOINIT_ATTR places in RTC slow memory which survives soft reset
#ifdef ESP32
RTC_NOINIT_ATTR
#endif
bool NimBLEPlatform::_unclean_shutdown = false;

//=============================================================================
// State Name Helpers (for logging)
//=============================================================================

const char* masterStateName(MasterState state) {
    switch (state) {
        case MasterState::IDLE: return "IDLE";
        case MasterState::SCAN_STARTING: return "SCAN_STARTING";
        case MasterState::SCANNING: return "SCANNING";
        case MasterState::SCAN_STOPPING: return "SCAN_STOPPING";
        case MasterState::CONN_STARTING: return "CONN_STARTING";
        case MasterState::CONNECTING: return "CONNECTING";
        case MasterState::CONN_CANCELING: return "CONN_CANCELING";
        default: return "UNKNOWN";
    }
}

const char* slaveStateName(SlaveState state) {
    switch (state) {
        case SlaveState::IDLE: return "IDLE";
        case SlaveState::ADV_STARTING: return "ADV_STARTING";
        case SlaveState::ADVERTISING: return "ADVERTISING";
        case SlaveState::ADV_STOPPING: return "ADV_STOPPING";
        default: return "UNKNOWN";
    }
}

const char* gapStateName(GAPState state) {
    switch (state) {
        case GAPState::UNINITIALIZED: return "UNINITIALIZED";
        case GAPState::INITIALIZING: return "INITIALIZING";
        case GAPState::READY: return "READY";
        case GAPState::MASTER_PRIORITY: return "MASTER_PRIORITY";
        case GAPState::SLAVE_PRIORITY: return "SLAVE_PRIORITY";
        case GAPState::TRANSITIONING: return "TRANSITIONING";
        case GAPState::ERROR_RECOVERY: return "ERROR_RECOVERY";
        default: return "UNKNOWN";
    }
}

//=============================================================================
// Constructor / Destructor
//=============================================================================

NimBLEPlatform::NimBLEPlatform() {
    // Initialize connection mutex
    _conn_mutex = xSemaphoreCreateMutex();
}

NimBLEPlatform::~NimBLEPlatform() {
    shutdown();
    if (_conn_mutex) {
        vSemaphoreDelete(_conn_mutex);
        _conn_mutex = nullptr;
    }
}

//=============================================================================
// Lifecycle
//=============================================================================

bool NimBLEPlatform::initialize(const PlatformConfig& config) {
    if (_initialized) {
        WARNING("NimBLEPlatform: Already initialized");
        return true;
    }

    _config = config;

    // Initialize NimBLE
    NimBLEDevice::init(_config.device_name);

    // Address type for ESP32-S3:
    // - BLE_OWN_ADDR_PUBLIC fails with error 13 (ETIMEOUT) for client connections
    // - BLE_OWN_ADDR_RPA_PUBLIC_DEFAULT also fails with error 13
    // - BLE_OWN_ADDR_RANDOM works for client connections
    // Using RANDOM address allows connections to work. Role negotiation is handled
    // by always initiating connections and using identity-based duplicate detection.
    NimBLEDevice::setOwnAddrType(BLE_OWN_ADDR_RANDOM);

    // Set power level (ESP32)
    NimBLEDevice::setPower(ESP_PWR_LVL_P9);

    // Set MTU
    NimBLEDevice::setMTU(_config.preferred_mtu);

    // Setup server (peripheral mode)
    if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
        if (!setupServer()) {
            ERROR("NimBLEPlatform: Failed to setup server");
            return false;
        }
    }

    // Setup scan (central mode)
    if (_config.role == Role::CENTRAL || _config.role == Role::DUAL) {
        if (!setupScan()) {
            ERROR("NimBLEPlatform: Failed to setup scan");
            return false;
        }
    }

    _initialized = true;

    // Set GAP state to READY
    portENTER_CRITICAL(&_state_mux);
    _gap_state = GAPState::READY;
    portEXIT_CRITICAL(&_state_mux);

    INFO("NimBLEPlatform: Initialized, role: " + std::string(roleToString(_config.role)));

    return true;
}

bool NimBLEPlatform::start() {
    if (!_initialized) {
        ERROR("NimBLEPlatform: Not initialized");
        return false;
    }

    if (_running) {
        return true;
    }

    // Start advertising if peripheral mode
    if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
        if (!startAdvertising()) {
            WARNING("NimBLEPlatform: Failed to start advertising");
        }
    }

    _running = true;
    INFO("NimBLEPlatform: Started");

    return true;
}

void NimBLEPlatform::stop() {
    if (!_running) {
        return;
    }

    stopScan();
    stopAdvertising();
    disconnectAll();

    _running = false;
    INFO("NimBLEPlatform: Stopped");
}

void NimBLEPlatform::loop() {
    if (!_running) {
        return;
    }

    // Process deferred disconnects from NimBLE host task callbacks.
    // Must run before other loop logic to ensure stale connections are cleaned up.
    processPendingDisconnects();

    // Process deferred error recovery (requested from callback context)
    if (_error_recovery_requested) {
        _error_recovery_requested = false;
        enterErrorRecovery();
    }

    // Check if continuous scan should stop
    portENTER_CRITICAL(&_state_mux);
    MasterState ms = _master_state;
    portEXIT_CRITICAL(&_state_mux);

    if (ms == MasterState::SCANNING && _scan_stop_time > 0 && millis() >= _scan_stop_time) {
        DEBUG("NimBLEPlatform: Stopping scan after timeout");
        stopScan();

        if (_on_scan_complete) {
            _on_scan_complete();
        }
    }

    // Stuck-state safety net: if GAP hardware is idle but our state machine
    // thinks we're busy, reset state machine. This recovers from missed callbacks
    // (e.g., service discovery disconnect not properly cleaning up state).
    // Skip during CONNECTING — connectNative() can legitimately take up to 10s.
    static uint32_t last_stuck_check = 0;
    uint32_t now_ms = millis();
    if (now_ms - last_stuck_check >= 5000) {  // Check every 5 seconds
        last_stuck_check = now_ms;

        portENTER_CRITICAL(&_state_mux);
        GAPState gs = _gap_state;
        MasterState ms2 = _master_state;
        SlaveState ss = _slave_state;
        portEXIT_CRITICAL(&_state_mux);

        // Don't fire stuck detector while a connection attempt is in progress
        if (ms2 == MasterState::CONNECTING || ms2 == MasterState::CONN_STARTING) {
            // Expected — connect can take several seconds
        } else {
            bool gap_idle = !ble_gap_disc_active() && !ble_gap_adv_active() && !ble_gap_conn_active();

            if (gap_idle && (gs != GAPState::READY || ms2 != MasterState::IDLE || ss != SlaveState::IDLE)) {
                WARNING(std::string("NimBLEPlatform: Stuck state detected - GAP idle but state=") +
                        gapStateName(gs) + " master=" + masterStateName(ms2) +
                        " slave=" + slaveStateName(ss) + ". Resetting.");
                portENTER_CRITICAL(&_state_mux);
                _gap_state = GAPState::READY;
                _master_state = MasterState::IDLE;
                _slave_state = SlaveState::IDLE;
                _slave_paused_for_master = false;
                portEXIT_CRITICAL(&_state_mux);

                // Restart advertising in dual/peripheral mode
                if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
                    startAdvertising();
                }
            }
        }
    }

    // Process operation queue
    BLEOperationQueue::process();
}

void NimBLEPlatform::shutdown() {
    // Guard re-entrant shutdown (e.g. recoverBLEStack -> shutdown -> callback -> recoverBLEStack -> shutdown)
    if (_shutting_down) {
        WARNING("NimBLEPlatform: Shutdown already in progress, skipping");
        return;
    }

    INFO("NimBLEPlatform: Beginning graceful shutdown");

    // Mark as shutting down FIRST to prevent:
    // 1. Re-entrant shutdown calls
    // 2. Callbacks from doing cleanup (onDisconnect would double-free clients)
    _shutting_down = true;

    // CONC-H4: Graceful shutdown timeout for active write operations
    const uint32_t SHUTDOWN_TIMEOUT_MS = 10000;
    uint32_t start = millis();

    // Stop accepting new operations by transitioning GAP state
    // This prevents new connections/operations from starting
    portENTER_CRITICAL(&_state_mux);
    GAPState current_gap = _gap_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_gap == GAPState::READY) {
        transitionGAPState(GAPState::READY, GAPState::TRANSITIONING);
    }

    // Wait for active write operations to complete
    while (hasActiveWriteOperations() && (millis() - start) < SHUTDOWN_TIMEOUT_MS) {
        DEBUG("NimBLEPlatform: Waiting for " + std::to_string(_active_write_count.load()) +
              " active write operation(s)");
        // DELAY RATIONALE: Shutdown wait polling - check every 100ms for write completion
        delay(100);
    }

    // Check if we timed out
    if (hasActiveWriteOperations()) {
        WARNING("NimBLEPlatform: Shutdown timeout (" +
                std::to_string(SHUTDOWN_TIMEOUT_MS) + "ms) with " +
                std::to_string(_active_write_count.load()) + " active writes - forcing close");
        _unclean_shutdown = true;
    } else {
        DEBUG("NimBLEPlatform: All operations complete, proceeding with clean shutdown");
    }

    // Stop advertising and scanning
    stop();

    // Notify higher layers about all disconnections BEFORE deinit,
    // so the peer manager can reset peer states properly.
    // Do NOT delete clients individually — deinit(true) handles all client cleanup.
    // Process any remaining deferred disconnects before shutdown cleanup
    processPendingDisconnects();

    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(1000))) {
        if (_on_disconnected) {
            for (auto& kv : _connections) {
                _on_disconnected(kv.second, 0x16);  // 0x16 = local host terminated
            }
        }
        _clients.clear();
        _connections.clear();
        _cached_rx_chars.clear();
        _discovered_devices.clear();
        _discovered_order.clear();
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform: Could not acquire mutex for cleanup - forcing cleanup");
        if (_on_disconnected) {
            for (auto& kv : _connections) {
                _on_disconnected(kv.second, 0x16);
            }
        }
        _clients.clear();
        _connections.clear();
        _cached_rx_chars.clear();
        _discovered_devices.clear();
        _discovered_order.clear();
    }

    // Deinit NimBLE stack — deinit(true) disconnects and deletes all clients/server.
    // We do NOT delete clients individually above to avoid double-free.
    if (_initialized) {
        NimBLEDevice::deinit(true);
        _initialized = false;
    }

    _server = nullptr;
    _service = nullptr;
    _rx_char = nullptr;
    _tx_char = nullptr;
    _identity_char = nullptr;
    _scan = nullptr;
    _advertising_obj = nullptr;

    _shutting_down = false;

    INFO("NimBLEPlatform: Shutdown complete" +
         std::string(wasCleanShutdown() ? "" : " (unclean - verify on boot)"));
}

bool NimBLEPlatform::isRunning() const {
    return _running;
}

//=============================================================================
// BLE Stack Recovery
//=============================================================================

bool NimBLEPlatform::recoverBLEStack() {
    // NimBLEDevice::deinit() frees memory that the NimBLE host task may have
    // corrupted during sync failures, causing CORRUPT HEAP panics. The only
    // safe recovery is a full reboot. With atomic file persistence, data
    // survives reboots reliably.
    ERROR("NimBLEPlatform: BLE stack stuck - persisting data and rebooting");

    // Persist any dirty data before reboot
    RNS::Identity::persist_data();

    delay(100);
    ESP.restart();
    return false;  // Won't reach here
}

bool NimBLEPlatform::attemptHostReset() {
    INFO("NimBLEPlatform: Attempting ble_hs_sched_reset (attempt " +
         std::to_string(_host_reset_attempts + 1) + ")");

    ble_hs_sched_reset(BLE_HS_ETIMEOUT);

    // Poll for resync up to 3s
    uint32_t start = millis();
    while (!ble_hs_synced() && (millis() - start) < 3000) {
        delay(50);
    }

    if (ble_hs_synced()) {
        unsigned long elapsed = millis() - start;
        INFO("NimBLEPlatform: Host resync successful after " +
             std::to_string(elapsed) + "ms");
        return true;
    }

    WARNING("NimBLEPlatform: Host resync failed after 3s");
    return false;
}

//=============================================================================
// State Machine Implementation
//=============================================================================

bool NimBLEPlatform::transitionMasterState(MasterState expected, MasterState new_state) {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    if (_master_state == expected) {
        _master_state = new_state;
        ok = true;
    }
    portEXIT_CRITICAL(&_state_mux);
    if (ok) {
        DEBUG("NimBLEPlatform: Master state: " + std::string(masterStateName(expected)) +
              " -> " + std::string(masterStateName(new_state)));
    }
    return ok;
}

bool NimBLEPlatform::transitionSlaveState(SlaveState expected, SlaveState new_state) {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    if (_slave_state == expected) {
        _slave_state = new_state;
        ok = true;
    }
    portEXIT_CRITICAL(&_state_mux);
    if (ok) {
        DEBUG("NimBLEPlatform: Slave state: " + std::string(slaveStateName(expected)) +
              " -> " + std::string(slaveStateName(new_state)));
    }
    return ok;
}

bool NimBLEPlatform::transitionGAPState(GAPState expected, GAPState new_state) {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    if (_gap_state == expected) {
        _gap_state = new_state;
        ok = true;
    }
    portEXIT_CRITICAL(&_state_mux);
    if (ok) {
        DEBUG("NimBLEPlatform: GAP state: " + std::string(gapStateName(expected)) +
              " -> " + std::string(gapStateName(new_state)));
    }
    return ok;
}

bool NimBLEPlatform::canStartScan() const {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    ok = (_gap_state == GAPState::READY || _gap_state == GAPState::MASTER_PRIORITY)
         && _master_state == MasterState::IDLE
         && !ble_gap_disc_active()
         && !ble_gap_conn_active();  // Also check no connection in progress
    portEXIT_CRITICAL(&_state_mux);
    return ok;
}

bool NimBLEPlatform::canStartAdvertising() const {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    ok = (_gap_state == GAPState::READY || _gap_state == GAPState::SLAVE_PRIORITY)
         && _slave_state == SlaveState::IDLE
         && !ble_gap_adv_active();
    portEXIT_CRITICAL(&_state_mux);
    return ok;
}

bool NimBLEPlatform::canConnect() const {
    bool ok = false;
    portENTER_CRITICAL(&_state_mux);
    ok = (_gap_state == GAPState::READY || _gap_state == GAPState::MASTER_PRIORITY)
         && _master_state == MasterState::IDLE
         && !ble_gap_conn_active();
    portEXIT_CRITICAL(&_state_mux);
    return ok;
}

bool NimBLEPlatform::pauseSlaveForMaster() {
    // Check if slave is currently advertising
    portENTER_CRITICAL(&_state_mux);
    SlaveState current_slave = _slave_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_slave == SlaveState::IDLE) {
        DEBUG("NimBLEPlatform: Slave already idle, no pause needed");
        return true;  // Already idle
    }

    if (current_slave == SlaveState::ADVERTISING) {
        // Transition to stopping
        if (!transitionSlaveState(SlaveState::ADVERTISING, SlaveState::ADV_STOPPING)) {
            WARNING("NimBLEPlatform: Failed to transition slave to ADV_STOPPING");
            return false;
        }

        // Stop advertising
        if (_advertising_obj) {
            _advertising_obj->stop();
        }

        // Also stop at low level
        if (ble_gap_adv_active()) {
            ble_gap_adv_stop();
        }

        // Wait for advertising to stop
        uint32_t start = millis();
        while (ble_gap_adv_active() && millis() - start < 2000) {
            // DELAY RATIONALE: Advertising stop polling - check completion every NimBLE scheduler tick (~10ms)
            delay(10);
        }

        if (ble_gap_adv_active()) {
            ERROR("NimBLEPlatform: Advertising didn't stop within 2s");
            // Force state to IDLE anyway
            portENTER_CRITICAL(&_state_mux);
            _slave_state = SlaveState::IDLE;
            portEXIT_CRITICAL(&_state_mux);
            return false;
        }

        // Transition to IDLE
        portENTER_CRITICAL(&_state_mux);
        _slave_state = SlaveState::IDLE;
        portEXIT_CRITICAL(&_state_mux);

        _slave_paused_for_master = true;
        DEBUG("NimBLEPlatform: Slave paused for master operation");
        return true;
    }

    // In other states (ADV_STARTING, ADV_STOPPING), wait for completion
    uint32_t start = millis();
    while (millis() - start < 2000) {
        portENTER_CRITICAL(&_state_mux);
        current_slave = _slave_state;
        portEXIT_CRITICAL(&_state_mux);

        if (current_slave == SlaveState::IDLE) {
            _slave_paused_for_master = true;
            return true;
        }
        // DELAY RATIONALE: Slave state polling - check completion every NimBLE scheduler tick (~10ms)
        delay(10);
    }

    WARNING("NimBLEPlatform: Timed out waiting for slave to become idle");
    return false;
}

void NimBLEPlatform::resumeSlave() {
    // Atomically check and clear the paused flag to prevent race conditions
    bool should_resume = false;
    portENTER_CRITICAL(&_state_mux);
    if (_slave_paused_for_master) {
        _slave_paused_for_master = false;
        should_resume = true;
    }
    portEXIT_CRITICAL(&_state_mux);

    if (!should_resume) {
        return;
    }

    // Only restart advertising if in peripheral/dual mode
    if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
        DEBUG("NimBLEPlatform: Resuming slave (restarting advertising)");
        startAdvertising();
    }
}

void NimBLEPlatform::enterErrorRecovery() {
    // Guard against recursive calls (recoverBLEStack -> start -> enterErrorRecovery)
    static bool in_recovery = false;
    if (in_recovery) {
        WARNING("NimBLEPlatform: Already in error recovery, skipping");
        return;
    }
    in_recovery = true;
    WARNING("NimBLEPlatform: Entering error recovery");

    // Reset all states atomically
    portENTER_CRITICAL(&_state_mux);
    _gap_state = GAPState::ERROR_RECOVERY;
    _master_state = MasterState::IDLE;
    _slave_state = SlaveState::IDLE;
    portEXIT_CRITICAL(&_state_mux);

    // Force stop all operations at low level first
    if (ble_gap_disc_active()) {
        ble_gap_disc_cancel();
    }
    if (ble_gap_conn_active()) {
        WARNING("NimBLEPlatform: Cancelling stuck GAP connection in error recovery");
        ble_gap_conn_cancel();
    }
    if (ble_gap_adv_active()) {
        ble_gap_adv_stop();
    }

    // Stop high level objects
    if (_scan) {
        _scan->stop();
    }
    if (_advertising_obj) {
        _advertising_obj->stop();
    }

    _scan_stop_time = 0;
    _slave_paused_for_master = false;

    // Wait for host to sync after any reset operation
    // Give the host up to 5s — NimBLE typically re-syncs within 1-3s
    if (!ble_hs_synced()) {
        WARNING("NimBLEPlatform: Host not synced, waiting up to 5s...");
        uint32_t sync_start = millis();
        while (!ble_hs_synced() && (millis() - sync_start) < 5000) {
            delay(50);
        }
        if (ble_hs_synced()) {
            INFO("NimBLEPlatform: Host sync restored after " +
                 std::to_string(millis() - sync_start) + "ms");
        } else {
            // Don't immediately reboot — track desync time and let startScan()
            // handle the reboot decision based on prolonged desync (30s).
            WARNING("NimBLEPlatform: Host not synced after 5s, will retry on next scan cycle");
            if (_host_desync_since == 0) {
                _host_desync_since = millis();
            }
            in_recovery = false;
            return;
        }
    }

    // Force host-controller resync to clear stale HCI state (fixes rc=530 / Invalid HCI params)
    // After a 574 desync, the controller's scan state can become corrupted even after host re-syncs.
    INFO("NimBLEPlatform: Scheduling host reset for controller resync");
    ble_hs_sched_reset(BLE_HS_ECONTROLLER);

    // Wait for host to re-sync after reset
    {
        uint32_t reset_start = millis();
        while (!ble_hs_synced() && (millis() - reset_start) < 5000) {
            delay(50);
        }
        if (ble_hs_synced()) {
            INFO("NimBLEPlatform: Host-controller resync after " +
                 std::to_string(millis() - reset_start) + "ms");
        } else {
            WARNING("NimBLEPlatform: Host-controller resync failed after 5s");
        }
    }

    // DELAY RATIONALE: Connect attempt recovery - ESP32-S3 settling time after host sync
    delay(100);

    // Re-acquire scan object to reset NimBLE internal state
    // This is necessary because NimBLE scan object can get into stuck state
    _scan = NimBLEDevice::getScan();
    if (_scan) {
        _scan->setScanCallbacks(this, false);
        _scan->setActiveScan(_config.scan_mode == ScanMode::ACTIVE);
        _scan->setInterval(_config.scan_interval_ms);
        _scan->setWindow(_config.scan_window_ms);
        _scan->setFilterPolicy(BLE_HCI_SCAN_FILT_NO_WL);
        _scan->setDuplicateFilter(true);
        _scan->clearResults();
    }

    // Verify GAP is truly idle
    if (!ble_gap_disc_active() && !ble_gap_adv_active() && !ble_gap_conn_active()) {
        portENTER_CRITICAL(&_state_mux);
        _gap_state = GAPState::READY;
        portEXIT_CRITICAL(&_state_mux);
        INFO("NimBLEPlatform: Error recovery complete, GAP ready");
    } else {
        ERROR("NimBLEPlatform: GAP still busy after recovery attempt");
    }

    // Restart advertising if in peripheral/dual mode
    if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
        DEBUG("NimBLEPlatform: Restarting advertising after recovery");
        startAdvertising();
    }

    in_recovery = false;
}

//=============================================================================
// Deferred Disconnect Processing
//=============================================================================

void NimBLEPlatform::queueDisconnect(uint16_t conn_handle, int reason, bool is_peripheral) {
    uint8_t next = (_pending_disc_write + 1) % PENDING_DISC_QUEUE_SIZE;
    if (next == _pending_disc_read) {
        // Queue full — more simultaneous disconnects than queue size
        WARNING("NimBLEPlatform: Pending disconnect queue full, dropping handle=" +
                std::to_string(conn_handle));
        return;
    }
    _pending_disc_queue[_pending_disc_write] = {conn_handle, reason, is_peripheral};
    _pending_disc_write = next;
}

void NimBLEPlatform::processPendingDisconnects() {
    while (_pending_disc_read != _pending_disc_write) {
        PendingDisconnect& pd = _pending_disc_queue[_pending_disc_read];

        // Hold _conn_mutex while modifying _connections/_clients/_cached_*_chars
        // to prevent races with write() called from the main loop task.
        ConnectionHandle conn;
        bool found = false;
        bool is_peripheral = pd.is_peripheral;

        NimBLEClient* client_to_delete = nullptr;

        // If GATT ops are in flight, defer this disconnect to avoid blocking
        // the loop task. It will be retried on the next iteration.
        if (hasActiveWriteOperations()) {
            break;
        }

        if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(100))) {
            // Re-check under mutex: a write() on the other core may have
            // called beginWriteOperation() between our check above and
            // this mutex acquisition.
            if (hasActiveWriteOperations()) {
                xSemaphoreGive(_conn_mutex);
                break;
            }
            auto conn_it = _connections.find(pd.conn_handle);
            if (conn_it != _connections.end()) {
                conn = conn_it->second;
                _connections.erase(conn_it);
                found = true;

                if (!pd.is_peripheral) {
                    // Central mode: remove from maps, defer client deletion
                    // until after mutex release to avoid use-after-free when
                    // write() holds a pointer to a child characteristic.
                    auto client_it = _clients.find(pd.conn_handle);
                    if (client_it != _clients.end()) {
                        client_to_delete = client_it->second;
                        _clients.erase(client_it);
                    }
                    _cached_rx_chars.erase(pd.conn_handle);
                    _cached_tx_chars.erase(pd.conn_handle);
                    _cached_identity_chars.erase(pd.conn_handle);
                }
            }
            xSemaphoreGive(_conn_mutex);
        } else {
            WARNING("NimBLEPlatform: Could not acquire mutex for disconnect processing");
            break;  // Retry next loop iteration
        }

        // Delete client AFTER releasing mutex. GATT ops were checked above,
        // so no in-flight operations should be holding child pointers.
        if (client_to_delete) {
            NimBLEDevice::deleteClient(client_to_delete);
        }

        if (found) {
            INFO("NimBLEPlatform: Processing deferred disconnect for " +
                 conn.peer_address.toString() + " reason=" + std::to_string(pd.reason));

            // Clear operation queue for this connection
            clearForConnection(pd.conn_handle);

            // Notify higher layers (outside mutex — callbacks may re-enter)
            if (is_peripheral) {
                if (_on_central_disconnected) {
                    _on_central_disconnected(conn);
                }
            } else {
                if (_on_disconnected) {
                    _on_disconnected(conn, static_cast<uint8_t>(pd.reason));
                }
            }

            // Restart advertising if in peripheral/dual mode
            if ((_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) &&
                !isAdvertising()) {
                startAdvertising();
            }
        }

        _pending_disc_read = (_pending_disc_read + 1) % PENDING_DISC_QUEUE_SIZE;
    }
}

//=============================================================================
// Central Mode - Scanning
//=============================================================================

bool NimBLEPlatform::startScan(uint16_t duration_ms) {
    if (!_scan) {
        ERROR("NimBLEPlatform: Scan not initialized");
        return false;
    }

    // Check current master state
    portENTER_CRITICAL(&_state_mux);
    MasterState current_master = _master_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_master == MasterState::SCANNING) {
        _scan_fail_count = 0;  // Reset on successful state
        return true;
    }

    // Wait for host sync before trying to scan (host may be resetting after connection failure).
    // NimBLE host self-recovers from most desyncs within 1-5s. Only reboot after prolonged desync.
    if (!ble_hs_synced()) {
        // Track when desync started
        if (_host_desync_since == 0) {
            _host_desync_since = millis();
        }

        DEBUG("NimBLEPlatform: Host not synced, waiting before scan...");
        uint32_t sync_wait = millis();
        while (!ble_hs_synced() && (millis() - sync_wait) < 3000) {
            delay(50);
        }
        if (!ble_hs_synced()) {
            unsigned long desync_duration = millis() - _host_desync_since;
            _scan_fail_count++;
            // Capture NimBLE's internal reset reason (set in patched onReset callback)
            int reset_reason = nimble_host_reset_reason;
            if (reset_reason != 0) {
                nimble_host_reset_reason = 0;
            }
            WARNING("NimBLEPlatform: Host not synced, desync " +
                    std::to_string(desync_duration / 1000) + "s (fail " +
                    std::to_string(_scan_fail_count) + "/" +
                    std::to_string(SCAN_FAIL_RECOVERY_THRESHOLD) +
                    ", resets=" + std::to_string(_host_reset_attempts) +
                    (reset_reason != 0 ? ", nimble_reason=" + std::to_string(reset_reason) : "") +
                    ")");

            // Tiered recovery:
            //   0-10s:  Wait for natural self-recovery
            //   10s:    Try ble_hs_sched_reset() (first attempt)
            //   30s:    Try ble_hs_sched_reset() (second attempt)
            //   60s+:   Reboot (last resort)
            if (desync_duration >= 10000 && _host_reset_attempts == 0) {
                _host_reset_attempts++;
                if (attemptHostReset()) {
                    _host_desync_since = 0;
                    _host_reset_attempts = 0;
                    _scan_fail_count = 0;
                    return false;  // Synced — will succeed on next scan cycle
                }
            } else if (desync_duration >= 30000 && _host_reset_attempts == 1) {
                _host_reset_attempts++;
                if (attemptHostReset()) {
                    _host_desync_since = 0;
                    _host_reset_attempts = 0;
                    _scan_fail_count = 0;
                    return false;
                }
            } else if (desync_duration >= HOST_DESYNC_REBOOT_MS) {
                ERROR("NimBLEPlatform: Host desynced for " +
                      std::to_string(desync_duration / 1000) + "s (conns=" +
                      std::to_string(getConnectionCount()) +
                      ", resets=" + std::to_string(_host_reset_attempts) +
                      "), rebooting");
                _scan_fail_count = 0;
                _host_desync_since = 0;
                _host_reset_attempts = 0;
                recoverBLEStack();
            }
            return false;
        }
    }

    // Host is synced — clear desync tracking
    if (_host_desync_since != 0) {
        unsigned long recovery_time = millis() - _host_desync_since;
        // Capture any remaining reset reason from NimBLE
        int reset_reason = nimble_host_reset_reason;
        if (reset_reason != 0) {
            nimble_host_reset_reason = 0;
        }
        INFO("NimBLEPlatform: Host re-synced after " + std::to_string(recovery_time) + "ms" +
             (reset_reason != 0 ? " (nimble_reason=" + std::to_string(reset_reason) + ")" : ""));
        _host_desync_since = 0;
        _host_reset_attempts = 0;
        _last_desync_recovery = millis();  // Start cooldown before allowing connections
    }

    // Log GAP hardware state before checking (INFO for UDP visibility during soak test)
    INFO("NimBLEPlatform: Pre-scan GAP: disc=" + std::to_string(ble_gap_disc_active()) +
         " adv=" + std::to_string(ble_gap_adv_active()) +
         " conn=" + std::to_string(ble_gap_conn_active()));

    // If a stale GAP connection is blocking scan, cancel it proactively
    if (ble_gap_conn_active() && _master_state == MasterState::IDLE) {
        WARNING("NimBLEPlatform: Stale GAP conn blocking scan - cancelling");
        ble_gap_conn_cancel();
        delay(50);  // Let GAP process the cancel
    }

    // Verify we can start scan
    if (!canStartScan()) {
        WARNING("NimBLEPlatform: Cannot start scan - state check failed" +
              std::string(" master=") + masterStateName(current_master) +
              " gap_disc=" + std::to_string(ble_gap_disc_active()) +
              " gap_conn=" + std::to_string(ble_gap_conn_active()));
        return false;
    }

    // Pause slave (advertising) for master operation
    if (!pauseSlaveForMaster()) {
        WARNING("NimBLEPlatform: Failed to pause slave for scan");
        // Try to restart advertising in case it was stopped but flag wasn't set
        if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
            startAdvertising();
        }
        return false;
    }

    // DELAY RATIONALE: MTU negotiation settling - allow stack to stabilize before scan start
    delay(20);

    // Transition to SCAN_STARTING
    if (!transitionMasterState(MasterState::IDLE, MasterState::SCAN_STARTING)) {
        WARNING("NimBLEPlatform: Failed to transition to SCAN_STARTING");
        resumeSlave();
        return false;
    }

    // Set GAP to master priority
    portENTER_CRITICAL(&_state_mux);
    _gap_state = GAPState::MASTER_PRIORITY;
    portEXIT_CRITICAL(&_state_mux);

    uint32_t duration_sec = (duration_ms == 0) ? 0 : (duration_ms / 1000);
    if (duration_sec < 1) duration_sec = 1;  // Minimum 1 second

    // Clear results and reconfigure scan before starting
    _scan->clearResults();
    _scan->setActiveScan(_config.scan_mode == ScanMode::ACTIVE);
    _scan->setInterval(_config.scan_interval_ms);
    _scan->setWindow(_config.scan_window_ms);

    DEBUG("NimBLEPlatform: Starting scan with duration=" + std::to_string(duration_sec) + "s");

    // NimBLE 2.x: use 0 for continuous scanning (we'll stop it manually in loop())
    bool started = _scan->start(0, false);

    if (started) {
        // Transition to SCANNING
        portENTER_CRITICAL(&_state_mux);
        _master_state = MasterState::SCANNING;
        portEXIT_CRITICAL(&_state_mux);

        _scan_fail_count = 0;
        _lightweight_reset_fails = 0;
        _scan_stop_time = millis() + duration_ms;
        INFO("BLE SCAN: Started, duration=" + std::to_string(duration_ms) + "ms");
        return true;
    }

    // Scan failed — log GAP state for diagnosis
    ERROR("NimBLEPlatform: Failed to start scan - GAP: disc=" + std::to_string(ble_gap_disc_active()) +
          " conn=" + std::to_string(ble_gap_conn_active()) +
          " adv=" + std::to_string(ble_gap_adv_active()) +
          " master=" + masterStateName(_master_state));

    // Reset state
    portENTER_CRITICAL(&_state_mux);
    _master_state = MasterState::IDLE;
    _gap_state = GAPState::READY;
    portEXIT_CRITICAL(&_state_mux);

    _scan_fail_count++;
    if (_scan_fail_count >= SCAN_FAIL_RECOVERY_THRESHOLD) {
        _scan_fail_count = 0;  // Reset so we don't immediately re-enter after recovery
        _lightweight_reset_fails++;

        if (_lightweight_reset_fails >= LIGHTWEIGHT_RESET_MAX_FAILS) {
            WARNING("NimBLEPlatform: " + std::to_string(_lightweight_reset_fails) +
                    " error recoveries failed to restore scan, escalating to full stack recovery");
            _lightweight_reset_fails = 0;
            recoverBLEStack();
        } else {
            WARNING("NimBLEPlatform: Too many scan failures, entering error recovery (" +
                    std::to_string(_lightweight_reset_fails) + "/" +
                    std::to_string(LIGHTWEIGHT_RESET_MAX_FAILS) + ")");
            enterErrorRecovery();
        }
    }

    resumeSlave();
    return false;
}

void NimBLEPlatform::stopScan() {
    portENTER_CRITICAL(&_state_mux);
    MasterState current_master = _master_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_master != MasterState::SCANNING && current_master != MasterState::SCAN_STARTING) {
        return;
    }

    // Transition to SCAN_STOPPING
    portENTER_CRITICAL(&_state_mux);
    _master_state = MasterState::SCAN_STOPPING;
    portEXIT_CRITICAL(&_state_mux);

    DEBUG("NimBLEPlatform: stopScan() called");

    if (_scan) {
        _scan->stop();
    }

    // Wait for scan to actually stop
    uint32_t start = millis();
    while (ble_gap_disc_active() && millis() - start < 1000) {
        // DELAY RATIONALE: Scan stop polling - check completion every NimBLE scheduler tick (~10ms)
        delay(10);
    }

    // Transition to IDLE
    portENTER_CRITICAL(&_state_mux);
    _master_state = MasterState::IDLE;
    _gap_state = GAPState::READY;
    portEXIT_CRITICAL(&_state_mux);

    _scan_stop_time = 0;
    DEBUG("NimBLEPlatform: Scan stopped");

    // Resume slave if it was paused
    resumeSlave();
}

bool NimBLEPlatform::isScanning() const {
    portENTER_CRITICAL(&_state_mux);
    bool scanning = (_master_state == MasterState::SCANNING ||
                     _master_state == MasterState::SCAN_STARTING);
    portEXIT_CRITICAL(&_state_mux);
    return scanning;
}

//=============================================================================
// Central Mode - Connections
//=============================================================================

bool NimBLEPlatform::connect(const BLEAddress& address, uint16_t timeout_ms) {
    NimBLEAddress nimAddr = toNimBLE(address);

    // Skip connections during desync cooldown — connecting while the NimBLE
    // stack is recovering from a desync can hang client->connect() (the host
    // task can't process the completion event), leading to WDT crashes.
    if (_host_desync_since != 0 || (_last_desync_recovery > 0 && millis() - _last_desync_recovery < DESYNC_CONNECT_COOLDOWN_MS)) {
        DEBUG("NimBLEPlatform: Skipping connect during desync cooldown");
        return false;
    }

    // Rate limit connections to avoid overwhelming the BLE stack
    // Non-blocking: return false if too soon, caller can retry later
    static unsigned long last_connect_time = 0;
    unsigned long now = millis();
    if (now - last_connect_time < 300) {  // Reduced from 500ms
        DEBUG("NimBLEPlatform: Connection rate limited, try again later");
        return false;  // Non-blocking: fail fast instead of delay
    }
    last_connect_time = millis();

    // Check if already connected
    if (isConnectedTo(address)) {
        WARNING("NimBLEPlatform: Already connected to " + address.toString());
        return false;
    }

    // Check connection limit
    if (getConnectionCount() >= _config.max_connections) {
        WARNING("NimBLEPlatform: Connection limit reached");
        return false;
    }

    // Verify we can connect using state machine
    if (!canConnect()) {
        portENTER_CRITICAL(&_state_mux);
        MasterState ms = _master_state;
        GAPState gs = _gap_state;
        portEXIT_CRITICAL(&_state_mux);
        WARNING("NimBLEPlatform: Cannot connect - state check failed" +
                std::string(" master=") + masterStateName(ms) +
                " gap=" + gapStateName(gs));
        return false;
    }

    // Stop scanning if active
    portENTER_CRITICAL(&_state_mux);
    MasterState current_master = _master_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_master == MasterState::SCANNING) {
        DEBUG("NimBLEPlatform: Stopping scan before connect");
        stopScan();
    }

    // Pause slave (advertising) for master operation
    if (!pauseSlaveForMaster()) {
        WARNING("NimBLEPlatform: Failed to pause slave for connect");
        // Try to restart advertising in case it was stopped but flag wasn't set
        if (_config.role == Role::PERIPHERAL || _config.role == Role::DUAL) {
            startAdvertising();
        }
        return false;
    }

    // Transition to CONN_STARTING
    if (!transitionMasterState(MasterState::IDLE, MasterState::CONN_STARTING)) {
        WARNING("NimBLEPlatform: Failed to transition to CONN_STARTING");
        resumeSlave();
        return false;
    }

    // Set GAP to master priority
    portENTER_CRITICAL(&_state_mux);
    _gap_state = GAPState::MASTER_PRIORITY;
    portEXIT_CRITICAL(&_state_mux);

    // DELAY RATIONALE: Service discovery settling - allow stack to finalize after advertising stop
    delay(20);

    // Verify GAP is truly idle
    if (ble_gap_disc_active() || ble_gap_adv_active()) {
        ERROR("NimBLEPlatform: GAP not idle before connect, entering error recovery");
        enterErrorRecovery();
        resumeSlave();
        return false;
    }

    // Check if there's still a pending connection
    if (ble_gap_conn_active()) {
        WARNING("NimBLEPlatform: Connection still pending in GAP, waiting...");
        uint32_t start = millis();
        while (ble_gap_conn_active() && millis() - start < 1000) {
            // DELAY RATIONALE: Service discovery polling - check completion per scheduler tick
            delay(10);
        }
        if (ble_gap_conn_active()) {
            ERROR("NimBLEPlatform: GAP connection still active after timeout");
            portENTER_CRITICAL(&_state_mux);
            _master_state = MasterState::IDLE;
            _gap_state = GAPState::READY;
            portEXIT_CRITICAL(&_state_mux);
            resumeSlave();
            return false;
        }
    }

    // Delete any existing clients for this address to ensure clean state
    NimBLEClient* existingClient = NimBLEDevice::getClientByPeerAddress(nimAddr);
    while (existingClient) {
        DEBUG("NimBLEPlatform: Deleting existing client for " + address.toString());
        if (existingClient->isConnected()) {
            existingClient->disconnect();
        }
        NimBLEDevice::deleteClient(existingClient);
        existingClient = NimBLEDevice::getClientByPeerAddress(nimAddr);
    }

    DEBUG("NimBLEPlatform: Connecting to " + address.toString() +
          " timeout=" + std::to_string(timeout_ms / 1000) + "s");

    // Transition to CONNECTING
    portENTER_CRITICAL(&_state_mux);
    _master_state = MasterState::CONNECTING;
    portEXIT_CRITICAL(&_state_mux);

    // Use native NimBLE connection
    bool connected = connectNative(address, timeout_ms);

    if (!connected) {
        ERROR("NimBLEPlatform: Native connection failed to " + address.toString());
        portENTER_CRITICAL(&_state_mux);
        _master_state = MasterState::IDLE;
        _gap_state = GAPState::READY;
        portEXIT_CRITICAL(&_state_mux);
        resumeSlave();
        return false;
    }

    // Connection succeeded - transition states
    portENTER_CRITICAL(&_state_mux);
    _master_state = MasterState::IDLE;
    _gap_state = GAPState::READY;
    portEXIT_CRITICAL(&_state_mux);

    // Remove from discovered devices cache
    std::string addrKey = nimAddr.toString().c_str();
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(100))) {
        auto cachedIt = _discovered_devices.find(addrKey);
        if (cachedIt != _discovered_devices.end()) {
            // Also remove from order tracking
            auto orderIt = std::find(_discovered_order.begin(),
                                      _discovered_order.end(), addrKey);
            if (orderIt != _discovered_order.end()) {
                _discovered_order.erase(orderIt);
            }
            _discovered_devices.erase(cachedIt);
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        // CONC-M5: Log timeout failures
        WARNING("NimBLEPlatform: conn_mutex timeout (100ms) during cache update");
    }

    DEBUG("NimBLEPlatform: Connection established successfully");

    // Resume slave operations
    resumeSlave();

    return true;
}

//=============================================================================
// Native NimBLE Connection (bypasses NimBLE-Arduino wrapper)
//=============================================================================

int NimBLEPlatform::nativeGapEventHandler(struct ble_gap_event* event, void* arg) {
    NimBLEPlatform* platform = static_cast<NimBLEPlatform*>(arg);

    switch (event->type) {
        case BLE_GAP_EVENT_CONNECT:
            DEBUG("NimBLEPlatform::nativeGapEventHandler: BLE_GAP_EVENT_CONNECT status=" +
                  std::to_string(event->connect.status) +
                  " handle=" + std::to_string(event->connect.conn_handle));

            platform->_native_connect_result = event->connect.status;
            if (event->connect.status == 0) {
                platform->_native_connect_success = true;
                platform->_native_connect_handle = event->connect.conn_handle;
                // Reset failure counters on successful connection
                platform->_conn_establish_fail_count = 0;
            } else {
                platform->_native_connect_success = false;
            }
            platform->_native_connect_pending = false;
            break;

        case BLE_GAP_EVENT_DISCONNECT: {
            uint16_t disc_handle = event->disconnect.conn.conn_handle;
            int disc_reason = event->disconnect.reason;

            DEBUG("NimBLEPlatform::nativeGapEventHandler: BLE_GAP_EVENT_DISCONNECT reason=" +
                  std::to_string(disc_reason) +
                  " handle=" + std::to_string(disc_handle));

            // If we were still waiting for connection, this is a failure
            if (platform->_native_connect_pending) {
                platform->_native_connect_result = disc_reason;
                platform->_native_connect_success = false;
                platform->_native_connect_pending = false;

                // Track connection establishment failures (574 = BLE_ERR_CONN_ESTABLISHMENT).
                // These commonly cause brief host desyncs that self-recover.
                // Don't escalate to enterErrorRecovery here — let the time-based
                // desync tracking in startScan() handle reboot decisions.
                if (disc_reason == 574) {
                    platform->_conn_establish_fail_count++;
                    WARNING("NimBLEPlatform: Connection establishment failed (574), count=" +
                            std::to_string(platform->_conn_establish_fail_count));
                }
            }

            // During shutdown, skip cleanup — shutdown() handles it
            if (platform->_shutting_down) {
                break;
            }

            // Defer map cleanup to BLE loop task to avoid data race.
            // This callback runs in the NimBLE host task while the BLE loop task
            // may be iterating _connections/_clients concurrently.
            platform->queueDisconnect(disc_handle, disc_reason, false);
            break;
        }

        default:
            DEBUG("NimBLEPlatform::nativeGapEventHandler: event type=" + std::to_string(event->type));
            break;
    }

    return 0;
}

bool NimBLEPlatform::connectNative(const BLEAddress& address, uint16_t timeout_ms) {
    INFO("NimBLEPlatform: Connecting to " + address.toString() + " type=" + std::to_string(address.type));

    // Verify host-controller sync — don't trigger recovery here,
    // just return false and let the host recover naturally. A single
    // connection failure (574) can cause a temporary host reset that
    // resolves on its own. Triggering recoverBLEStack() here would
    // kill all existing connections unnecessarily.
    if (!ble_hs_synced()) {
        WARNING("NimBLEPlatform: Host not synced before connect, skipping");
        return false;
    }

    if (address.type > 3) {
        ERROR("NimBLEPlatform: Invalid address type " + std::to_string(address.type));
        return false;
    }

    // Convert to NimBLE address
    NimBLEAddress nimAddr = toNimBLE(address);

    // Use NimBLEClient for connection — this properly manages the GAP event handler,
    // connection handle tracking, and service discovery. Raw ble_gap_connect() bypasses
    // NimBLE's internal client management, causing service discovery to fail.
    NimBLEClient* client = NimBLEDevice::createClient(nimAddr);
    if (!client) {
        ERROR("NimBLEPlatform: Failed to create NimBLE client");
        return false;
    }

    client->setClientCallbacks(this, false);
    client->setConnectionParams(24, 48, 0, 400);  // 30-60ms interval, 4.0s supervision timeout
    client->setConnectTimeout(timeout_ms);  // milliseconds

    // Suppress _on_connected in onConnect callback — we'll fire it from here
    // after connect() returns. The onConnect callback runs in the NimBLE host
    // task, and _on_connected triggers blocking GATT operations (service
    // discovery) that would deadlock the host task.
    _native_connect_pending = true;

    // Connect (blocking) — NimBLE handles GAP event management internally
    bool connected = client->connect(nimAddr, false);  // deleteAttributes=false

    _native_connect_pending = false;

    if (!connected) {
        INFO("NimBLEPlatform: Connection failed to " + address.toString());
        NimBLEDevice::deleteClient(client);
        return false;
    }

    // onConnect callback already stored in _connections/_clients.
    // Update MTU (exchange happens after onConnect fires).
    uint16_t conn_handle = client->getConnHandle();
    uint16_t negotiated_mtu = client->getMTU() - MTU::ATT_OVERHEAD;

    auto conn_it = _connections.find(conn_handle);
    if (conn_it != _connections.end()) {
        conn_it->second.mtu = negotiated_mtu;
    }

    INFO("NimBLEPlatform: Connected to " + address.toString() +
         " handle=" + std::to_string(conn_handle) +
         " MTU=" + std::to_string(negotiated_mtu));

    // Fire _on_connected from THIS task (BLEInterface loop), not the host task.
    // This allows the callback to safely do blocking GATT operations
    // (service discovery, notification enable, identity read/write).
    if (_on_connected) {
        ConnectionHandle conn = getConnection(conn_handle);
        _on_connected(conn);
    }

    return true;
}

bool NimBLEPlatform::disconnect(uint16_t conn_handle) {
    auto conn_it = _connections.find(conn_handle);
    if (conn_it == _connections.end()) {
        return false;
    }

    ConnectionHandle& conn = conn_it->second;

    if (conn.local_role == Role::CENTRAL) {
        // We are central - disconnect client
        auto client_it = _clients.find(conn_handle);
        if (client_it != _clients.end() && client_it->second) {
            client_it->second->disconnect();
            return true;
        }
    } else {
        // We are peripheral - disconnect via server
        if (_server) {
            _server->disconnect(conn_handle);
            return true;
        }
    }

    return false;
}

void NimBLEPlatform::disconnectAll() {
    // Disconnect all clients (central mode)
    for (auto& kv : _clients) {
        if (kv.second && kv.second->isConnected()) {
            kv.second->disconnect();
        }
    }

    // Disconnect all server connections (peripheral mode)
    if (_server) {
        std::vector<uint16_t> handles;
        for (const auto& kv : _connections) {
            if (kv.second.local_role == Role::PERIPHERAL) {
                handles.push_back(kv.first);
            }
        }
        for (uint16_t handle : handles) {
            _server->disconnect(handle);
        }
    }
}

bool NimBLEPlatform::requestMTU(uint16_t conn_handle, uint16_t mtu) {
    auto client_it = _clients.find(conn_handle);
    if (client_it == _clients.end() || !client_it->second) {
        return false;
    }

    // NimBLE handles MTU exchange automatically, but we can try to update
    // The MTU change callback will be invoked
    return true;
}

bool NimBLEPlatform::discoverServices(uint16_t conn_handle) {
    if (!ble_hs_synced()) {
        return false;
    }

    NimBLEClient* client = nullptr;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(100))) {
        auto client_it = _clients.find(conn_handle);
        if (client_it != _clients.end()) {
            client = client_it->second;
        }
        xSemaphoreGive(_conn_mutex);
    }
    if (!client) {
        return false;
    }

    // Get our service — blocking GATT operation
    NimBLERemoteService* service = client->getService(UUID::SERVICE);
    if (!service) {
        ERROR("NimBLEPlatform: Service not found");
        if (_on_services_discovered) {
            ConnectionHandle conn = getConnection(conn_handle);
            _on_services_discovered(conn, false);
        }
        return false;
    }

    // Get characteristics — each is a blocking GATT operation
    NimBLERemoteCharacteristic* rxChar = service->getCharacteristic(UUID::RX_CHAR);
    NimBLERemoteCharacteristic* txChar = service->getCharacteristic(UUID::TX_CHAR);
    NimBLERemoteCharacteristic* idChar = service->getCharacteristic(UUID::IDENTITY_CHAR);

    if (!rxChar || !txChar) {
        ERROR("NimBLEPlatform: Required characteristics not found");
        if (_on_services_discovered) {
            ConnectionHandle conn = getConnection(conn_handle);
            _on_services_discovered(conn, false);
        }
        return false;
    }

    // Update connection with characteristic handles and cache char pointers
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(200))) {
        auto conn_it = _connections.find(conn_handle);
        if (conn_it != _connections.end()) {
            conn_it->second.rx_char_handle = rxChar->getHandle();
            conn_it->second.tx_char_handle = txChar->getHandle();
            if (idChar) {
                conn_it->second.identity_handle = idChar->getHandle();
            }
            conn_it->second.state = ConnectionState::READY;
            // Only cache if connection still exists — if it was deleted
            // during blocking discovery, caching would leave dangling pointers.
            _cached_rx_chars[conn_handle] = rxChar;
            _cached_tx_chars[conn_handle] = txChar;
            if (idChar) {
                _cached_identity_chars[conn_handle] = idChar;
            }
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::discoverServices: mutex timeout, handle=" +
                std::to_string(conn_handle));
        if (_on_services_discovered) {
            ConnectionHandle conn;
            conn.handle = conn_handle;
            _on_services_discovered(conn, false);
        }
        return false;
    }

    DEBUG("NimBLEPlatform: Services discovered for " + std::to_string(conn_handle));

    if (_on_services_discovered) {
        ConnectionHandle conn = getConnection(conn_handle);
        _on_services_discovered(conn, true);
    }

    return true;
}

//=============================================================================
// Peripheral Mode
//=============================================================================

bool NimBLEPlatform::startAdvertising() {
    if (!_advertising_obj) {
        if (!setupAdvertising()) {
            return false;
        }
    }

    // Check current slave state
    portENTER_CRITICAL(&_state_mux);
    SlaveState current_slave = _slave_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_slave == SlaveState::ADVERTISING) {
        return true;
    }

    // Wait for host sync before advertising (host may be resetting)
    if (!ble_hs_synced()) {
        uint32_t sync_wait = millis();
        while (!ble_hs_synced() && (millis() - sync_wait) < 1000) {
            delay(50);
        }
        if (!ble_hs_synced()) {
            DEBUG("NimBLEPlatform: Host not synced, cannot start advertising");
            return false;
        }
    }

    // Check if we can start advertising
    if (!canStartAdvertising()) {
        DEBUG("NimBLEPlatform: Cannot start advertising - state check failed" +
              std::string(" slave=") + slaveStateName(current_slave) +
              " gap_adv=" + std::to_string(ble_gap_adv_active()));
        return false;
    }

    // Transition to ADV_STARTING
    if (!transitionSlaveState(SlaveState::IDLE, SlaveState::ADV_STARTING)) {
        WARNING("NimBLEPlatform: Failed to transition to ADV_STARTING");
        return false;
    }

    if (_advertising_obj->start()) {
        // Transition to ADVERTISING
        portENTER_CRITICAL(&_state_mux);
        _slave_state = SlaveState::ADVERTISING;
        portEXIT_CRITICAL(&_state_mux);

        DEBUG("NimBLEPlatform: Advertising started");
        return true;
    }

    // Failed to start
    portENTER_CRITICAL(&_state_mux);
    _slave_state = SlaveState::IDLE;
    portEXIT_CRITICAL(&_state_mux);

    ERROR("NimBLEPlatform: Failed to start advertising");
    return false;
}

void NimBLEPlatform::stopAdvertising() {
    portENTER_CRITICAL(&_state_mux);
    SlaveState current_slave = _slave_state;
    portEXIT_CRITICAL(&_state_mux);

    if (current_slave != SlaveState::ADVERTISING && current_slave != SlaveState::ADV_STARTING) {
        return;
    }

    // Transition to ADV_STOPPING
    portENTER_CRITICAL(&_state_mux);
    _slave_state = SlaveState::ADV_STOPPING;
    portEXIT_CRITICAL(&_state_mux);

    DEBUG("NimBLEPlatform: stopAdvertising() called");

    if (_advertising_obj) {
        _advertising_obj->stop();
    }

    // Also stop at low level
    if (ble_gap_adv_active()) {
        ble_gap_adv_stop();
    }

    // Wait for advertising to actually stop
    uint32_t start = millis();
    while (ble_gap_adv_active() && millis() - start < 1000) {
        // DELAY RATIONALE: Loop iteration throttle - prevent tight loop CPU consumption
        delay(10);
    }

    // Transition to IDLE
    portENTER_CRITICAL(&_state_mux);
    _slave_state = SlaveState::IDLE;
    portEXIT_CRITICAL(&_state_mux);

    DEBUG("NimBLEPlatform: Advertising stopped");
}

bool NimBLEPlatform::isAdvertising() const {
    portENTER_CRITICAL(&_state_mux);
    bool advertising = (_slave_state == SlaveState::ADVERTISING ||
                        _slave_state == SlaveState::ADV_STARTING);
    portEXIT_CRITICAL(&_state_mux);
    return advertising;
}

bool NimBLEPlatform::setAdvertisingData(const Bytes& data) {
    // Custom advertising data not directly supported by high-level API
    // Use the service UUID instead
    return true;
}

void NimBLEPlatform::setIdentityData(const Bytes& identity) {
    _identity_data = identity;

    if (_identity_char && identity.size() > 0) {
        _identity_char->setValue(identity.data(), identity.size());
        DEBUG("NimBLEPlatform: Identity data set");
    }

    // Update device name to include identity prefix (Protocol v2.2)
    // Format: "RNS-" + first 3 bytes of identity as hex (6 chars)
    // This allows peers to recognize us across MAC rotations
    if (identity.size() >= 3 && _advertising_obj) {
        char name[11];  // "RNS-" (4) + 6 hex chars + null
        snprintf(name, sizeof(name), "RNS-%02x%02x%02x",
                 identity.data()[0], identity.data()[1], identity.data()[2]);

        _advertising_obj->setName(name);
        DEBUG("NimBLEPlatform: Updated advertised name to " + std::string(name));

        // Restart advertising if currently active to apply new name
        if (isAdvertising()) {
            stopAdvertising();
            startAdvertising();
        }
    }
}

//=============================================================================
// GATT Operations
//=============================================================================

bool NimBLEPlatform::write(uint16_t conn_handle, const Bytes& data, bool response) {
    // Guard against use-after-free: during a host reset, NimBLE invalidates
    // client objects on core 0 while we may still hold stale pointers.
    if (!ble_hs_synced()) {
        return false;
    }

    // Resolve characteristic pointer under _conn_mutex, then release before
    // the blocking writeValue() call.  This prevents a race with
    // processPendingDisconnects() which erases from these maps on the BLE task
    // while send_outgoing() calls write() from the main loop task.
    NimBLERemoteCharacteristic* rxChar = nullptr;
    {
        if (!xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
            DEBUG("NimBLEPlatform::write: could not acquire mutex");
            return false;
        }

        auto conn_it = _connections.find(conn_handle);
        if (conn_it == _connections.end()) {
            xSemaphoreGive(_conn_mutex);
            DEBUG("NimBLEPlatform::write: no connection for handle " + std::to_string(conn_handle));
            return false;
        }

        ConnectionHandle& conn = conn_it->second;

        if (conn.local_role != Role::CENTRAL) {
            xSemaphoreGive(_conn_mutex);
            WARNING("NimBLEPlatform: write() called in peripheral mode, use notify()");
            return false;
        }

        auto client_it = _clients.find(conn_handle);
        if (client_it == _clients.end() || !client_it->second) {
            xSemaphoreGive(_conn_mutex);
            WARNING("NimBLEPlatform::write: no client for handle " + std::to_string(conn_handle));
            return false;
        }

        NimBLEClient* client = client_it->second;
        if (!client->isConnected()) {
            xSemaphoreGive(_conn_mutex);
            WARNING("NimBLEPlatform::write: client not connected for handle " + std::to_string(conn_handle));
            return false;
        }

        // Use cached RX characteristic pointer (populated by discoverServices())
        auto cached_it = _cached_rx_chars.find(conn_handle);
        if (cached_it != _cached_rx_chars.end()) {
            rxChar = cached_it->second;
        }

        // Register active op BEFORE releasing mutex so processPendingDisconnects()
        // sees it when checking hasActiveWriteOperations() — closes TOCTOU gap.
        if (rxChar) {
            beginWriteOperation();
        }

        xSemaphoreGive(_conn_mutex);
    }

    if (!rxChar) {
        WARNING("NimBLEPlatform::write: RX char not cached for handle " + std::to_string(conn_handle) +
                " (discoverServices() not yet called or failed?)");
        return false;
    }

    // writeValue() is a blocking GATT op — must NOT hold _conn_mutex here
    bool result = rxChar->writeValue(data.data(), data.size(), response);
    endWriteOperation();
    if (!result) {
        WARNING("NimBLEPlatform::write: writeValue failed for handle " + std::to_string(conn_handle));
    }
    return result;
}

bool NimBLEPlatform::writeCharacteristic(uint16_t conn_handle, uint16_t char_handle,
                                          const Bytes& data, bool response) {
    if (!ble_hs_synced()) {
        return false;
    }

    // Resolve pointers under _conn_mutex using cached chars, release before blocking writeValue()
    NimBLERemoteCharacteristic* chr = nullptr;
    {
        if (!xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
            return false;
        }

        auto client_it = _clients.find(conn_handle);
        if (client_it == _clients.end() || !client_it->second) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }

        NimBLEClient* client = client_it->second;
        if (!client->isConnected()) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }

        // Use cached characteristic pointers — populated after service discovery
        auto conn_it = _connections.find(conn_handle);
        if (conn_it != _connections.end() && char_handle == conn_it->second.identity_handle) {
            auto id_it = _cached_identity_chars.find(conn_handle);
            if (id_it != _cached_identity_chars.end()) {
                chr = id_it->second;
            }
        }
        if (!chr) {
            auto rx_it = _cached_rx_chars.find(conn_handle);
            if (rx_it != _cached_rx_chars.end()) {
                chr = rx_it->second;
            }
        }

        if (chr) {
            beginWriteOperation();
        }

        xSemaphoreGive(_conn_mutex);
    }

    if (!chr) return false;

    bool result = chr->writeValue(data.data(), data.size(), response);
    endWriteOperation();
    return result;
}

bool NimBLEPlatform::read(uint16_t conn_handle, uint16_t char_handle,
                          std::function<void(OperationResult, const Bytes&)> callback) {
    if (!ble_hs_synced()) {
        if (callback) callback(OperationResult::DISCONNECTED, Bytes());
        return false;
    }

    // Resolve pointers under _conn_mutex using cached chars, release before blocking readValue()
    NimBLERemoteCharacteristic* chr = nullptr;
    {
        if (!xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
            if (callback) callback(OperationResult::NOT_FOUND, Bytes());
            return false;
        }

        auto client_it = _clients.find(conn_handle);
        if (client_it == _clients.end() || !client_it->second) {
            xSemaphoreGive(_conn_mutex);
            if (callback) callback(OperationResult::NOT_FOUND, Bytes());
            return false;
        }

        NimBLEClient* client = client_it->second;
        if (!client->isConnected()) {
            xSemaphoreGive(_conn_mutex);
            if (callback) callback(OperationResult::DISCONNECTED, Bytes());
            return false;
        }

        // Use cached identity characteristic pointer
        auto conn_it = _connections.find(conn_handle);
        if (conn_it != _connections.end() && char_handle == conn_it->second.identity_handle) {
            auto id_it = _cached_identity_chars.find(conn_handle);
            if (id_it != _cached_identity_chars.end()) {
                chr = id_it->second;
            }
        }

        if (chr) {
            beginWriteOperation();
        }

        xSemaphoreGive(_conn_mutex);
    }

    if (!chr) {
        if (callback) callback(OperationResult::NOT_FOUND, Bytes());
        return false;
    }

    NimBLEAttValue value = chr->readValue();
    endWriteOperation();
    if (callback) {
        Bytes result(value.data(), value.size());
        callback(OperationResult::SUCCESS, result);
    }

    return true;
}

bool NimBLEPlatform::enableNotifications(uint16_t conn_handle, bool enable) {
    if (!ble_hs_synced()) {
        return false;
    }

    // Resolve pointers under _conn_mutex using cached chars, release before blocking subscribe()
    NimBLERemoteCharacteristic* txChar = nullptr;
    BLEAddress expected_peer;
    {
        if (!xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
            return false;
        }

        auto client_it = _clients.find(conn_handle);
        if (client_it == _clients.end() || !client_it->second) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }

        NimBLEClient* client = client_it->second;
        if (!client->isConnected()) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }

        // Use cached TX characteristic pointer
        auto tx_it = _cached_tx_chars.find(conn_handle);
        if (tx_it != _cached_tx_chars.end()) {
            txChar = tx_it->second;
        }
        if (!txChar) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }

        auto conn_it = _connections.find(conn_handle);
        if (conn_it == _connections.end()) {
            xSemaphoreGive(_conn_mutex);
            return false;
        }
        expected_peer = conn_it->second.peer_address;

        beginWriteOperation();
        xSemaphoreGive(_conn_mutex);
    }

    if (enable) {
        // Subscribe to notifications.
        // Capture peer_address to guard against conn_handle reuse: if peer A disconnects
        // (handle=1) and peer B connects (handle=1), we must not deliver B's data as A's.
        auto notifyCb = [this, conn_handle, expected_peer](NimBLERemoteCharacteristic* pChar,
                                             uint8_t* pData, size_t length, bool isNotify) {
            if (_on_data_received) {
                ConnectionHandle conn = getConnection(conn_handle);
                if (!conn.isValid() || conn.peer_address != expected_peer) {
                    return;  // Stale handle — peer changed
                }
                Bytes data(pData, length);
                _on_data_received(conn, data);
            }
        };

        bool result = txChar->subscribe(true, notifyCb);
        endWriteOperation();
        return result;
    } else {
        bool result = txChar->unsubscribe();
        endWriteOperation();
        return result;
    }
}

bool NimBLEPlatform::notify(uint16_t conn_handle, const Bytes& data) {
    if (!ble_hs_synced() || !_tx_char) {
        return false;
    }

    _tx_char->setValue(data.data(), data.size());
    return _tx_char->notify(true);
}

bool NimBLEPlatform::notifyAll(const Bytes& data) {
    if (!ble_hs_synced() || !_tx_char) {
        return false;
    }

    _tx_char->setValue(data.data(), data.size());
    return _tx_char->notify(true);  // Notifies all subscribed clients
}

//=============================================================================
// Connection Management
//=============================================================================

std::vector<ConnectionHandle> NimBLEPlatform::getConnections() const {
    std::vector<ConnectionHandle> result;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        for (const auto& kv : _connections) {
            result.push_back(kv.second);
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::getConnections: mutex timeout");
    }
    return result;
}

ConnectionHandle NimBLEPlatform::getConnection(uint16_t handle) const {
    ConnectionHandle result;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        auto it = _connections.find(handle);
        if (it != _connections.end()) {
            result = it->second;
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::getConnection: mutex timeout for handle " + std::to_string(handle));
    }
    return result;
}

size_t NimBLEPlatform::getConnectionCount() const {
    size_t count = 0;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        count = _connections.size();
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::getConnectionCount: mutex timeout");
    }
    return count;
}

bool NimBLEPlatform::isConnectedTo(const BLEAddress& address) const {
    bool found = false;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        for (const auto& kv : _connections) {
            if (kv.second.peer_address == address) {
                found = true;
                break;
            }
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::isConnectedTo: mutex timeout");
    }
    return found;
}

bool NimBLEPlatform::isDeviceConnected(const std::string& addrKey) const {
    bool found = false;
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        for (const auto& kv : _connections) {
            if (kv.second.peer_address.toString() == addrKey) {
                found = true;
                break;
            }
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::isDeviceConnected: mutex timeout");
    }
    return found;
}

//=============================================================================
// Callback Registration
//=============================================================================

void NimBLEPlatform::setOnScanResult(Callbacks::OnScanResult callback) {
    _on_scan_result = callback;
}

void NimBLEPlatform::setOnScanComplete(Callbacks::OnScanComplete callback) {
    _on_scan_complete = callback;
}

void NimBLEPlatform::setOnConnected(Callbacks::OnConnected callback) {
    _on_connected = callback;
}

void NimBLEPlatform::setOnDisconnected(Callbacks::OnDisconnected callback) {
    _on_disconnected = callback;
}

void NimBLEPlatform::setOnMTUChanged(Callbacks::OnMTUChanged callback) {
    _on_mtu_changed = callback;
}

void NimBLEPlatform::setOnServicesDiscovered(Callbacks::OnServicesDiscovered callback) {
    _on_services_discovered = callback;
}

void NimBLEPlatform::setOnDataReceived(Callbacks::OnDataReceived callback) {
    _on_data_received = callback;
}

void NimBLEPlatform::setOnNotifyEnabled(Callbacks::OnNotifyEnabled callback) {
    _on_notify_enabled = callback;
}

void NimBLEPlatform::setOnCentralConnected(Callbacks::OnCentralConnected callback) {
    _on_central_connected = callback;
}

void NimBLEPlatform::setOnCentralDisconnected(Callbacks::OnCentralDisconnected callback) {
    _on_central_disconnected = callback;
}

void NimBLEPlatform::setOnWriteReceived(Callbacks::OnWriteReceived callback) {
    _on_write_received = callback;
}

void NimBLEPlatform::setOnReadRequested(Callbacks::OnReadRequested callback) {
    _on_read_requested = callback;
}

BLEAddress NimBLEPlatform::getLocalAddress() const {
    // Try NimBLE's address first (uses configured own_addr_type)
    BLEAddress addr = fromNimBLE(NimBLEDevice::getAddress());
    if (!addr.isZero()) {
        return addr;
    }

    // Fallback: try ble_hs_id_copy_addr directly with RANDOM type
    uint8_t nimble_addr[6] = {};
    int rc = ble_hs_id_copy_addr(BLE_OWN_ADDR_RANDOM, nimble_addr, nullptr);
    if (rc == 0) {
        // NimBLE stores in little-endian: val[0]=LSB, val[5]=MSB
        BLEAddress result;
        for (int i = 0; i < 6; i++) {
            result.addr[i] = nimble_addr[5 - i];
        }
        if (!result.isZero()) return result;
    }

    // Fallback: read BT MAC directly from ESP-IDF efuse
    uint8_t mac[6] = {};
    esp_err_t err = esp_read_mac(mac, ESP_MAC_BT);
    if (err == ESP_OK) {
        // esp_read_mac returns in standard order: mac[0]=MSB (OUI), mac[5]=LSB
        // Our BLEAddress also stores MSB first, so direct copy
        BLEAddress result;
        memcpy(result.addr, mac, 6);
        if (!result.isZero()) return result;
    }

    WARNING(std::string("NimBLEPlatform::getLocalAddress: all methods failed") +
            " nimble_addr=" + addr.toString() +
            " ble_hs_id_copy_addr_rc=" + std::to_string(rc) +
            " esp_read_mac_rc=" + std::to_string(static_cast<int>(err)));
    return addr;
}

//=============================================================================
// NimBLE Server Callbacks (Peripheral mode)
//=============================================================================

void NimBLEPlatform::onConnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo) {
    uint16_t conn_handle = connInfo.getConnHandle();

    ConnectionHandle conn;
    conn.handle = conn_handle;
    conn.peer_address = fromNimBLE(connInfo.getAddress());
    conn.local_role = Role::PERIPHERAL;  // We are peripheral, they are central
    conn.state = ConnectionState::CONNECTED;
    conn.mtu = MTU::MINIMUM - MTU::ATT_OVERHEAD;

    // Read connection RSSI
    int8_t rssi_val = 0;
    if (ble_gap_conn_rssi(conn_handle, &rssi_val) == 0) {
        conn.rssi = rssi_val;
    }

    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(100))) {
        _connections[conn_handle] = conn;
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform: onConnect(server): mutex timeout, handle=" +
                std::to_string(conn_handle) + " not tracked");
    }

    DEBUG("NimBLEPlatform: Central connected: " + conn.peer_address.toString() +
          " rssi=" + std::to_string(conn.rssi));

    if (_on_central_connected) {
        _on_central_connected(conn);
    }

    // Continue advertising to accept more connections
    if (_config.role == Role::DUAL && getConnectionCount() < _config.max_connections) {
        startAdvertising();
    }
}

void NimBLEPlatform::onDisconnect(NimBLEServer* pServer, NimBLEConnInfo& connInfo, int reason) {
    if (_shutting_down) return;  // shutdown() handles cleanup

    uint16_t conn_handle = connInfo.getConnHandle();

    DEBUG("NimBLEPlatform: Central disconnect event for handle=" + std::to_string(conn_handle) +
          " reason=" + std::to_string(reason));

    // Defer map cleanup to BLE loop task to avoid data race.
    // This callback runs in the NimBLE host task while the BLE loop task
    // may be iterating _connections concurrently.
    queueDisconnect(conn_handle, reason, true);
}

void NimBLEPlatform::onMTUChange(uint16_t MTU, NimBLEConnInfo& connInfo) {
    uint16_t conn_handle = connInfo.getConnHandle();
    updateConnectionMTU(conn_handle, MTU);

    DEBUG("NimBLEPlatform: MTU changed to " + std::to_string(MTU) +
          " for connection " + std::to_string(conn_handle));

    if (_on_mtu_changed) {
        ConnectionHandle conn = getConnection(conn_handle);
        _on_mtu_changed(conn, MTU);
    }
}

//=============================================================================
// NimBLE Characteristic Callbacks
//=============================================================================

void NimBLEPlatform::onWrite(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) {
    uint16_t conn_handle = connInfo.getConnHandle();

    NimBLEAttValue value = pCharacteristic->getValue();
    Bytes data(value.data(), value.size());

    DEBUG("NimBLEPlatform::onWrite: Received " + std::to_string(data.size()) + " bytes from conn " + std::to_string(conn_handle));

    if (_on_write_received) {
        DEBUG("NimBLEPlatform::onWrite: Getting connection handle");
        ConnectionHandle conn = getConnection(conn_handle);
        DEBUG("NimBLEPlatform::onWrite: Calling callback, peer=" + conn.peer_address.toString());
        _on_write_received(conn, data);
        DEBUG("NimBLEPlatform::onWrite: Callback returned");
    } else {
        DEBUG("NimBLEPlatform::onWrite: No callback registered");
    }
}

void NimBLEPlatform::onRead(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo) {
    // Identity characteristic read - return stored identity
    if (pCharacteristic == _identity_char && _identity_data.size() > 0) {
        pCharacteristic->setValue(_identity_data.data(), _identity_data.size());
    }
}

void NimBLEPlatform::onSubscribe(NimBLECharacteristic* pCharacteristic, NimBLEConnInfo& connInfo,
                                  uint16_t subValue) {
    uint16_t conn_handle = connInfo.getConnHandle();
    bool enabled = (subValue > 0);

    DEBUG("NimBLEPlatform: Notifications " + std::string(enabled ? "enabled" : "disabled") +
          " for connection " + std::to_string(conn_handle));

    if (_on_notify_enabled) {
        ConnectionHandle conn = getConnection(conn_handle);
        _on_notify_enabled(conn, enabled);
    }
}

//=============================================================================
// NimBLE Client Callbacks (Central mode)
//=============================================================================

void NimBLEPlatform::onConnect(NimBLEClient* pClient) {
    uint16_t conn_handle = pClient->getConnHandle();
    BLEAddress peer_addr = fromNimBLE(pClient->getPeerAddress());

    ConnectionHandle conn;
    conn.handle = conn_handle;
    conn.peer_address = peer_addr;
    conn.local_role = Role::CENTRAL;  // We are central
    conn.state = ConnectionState::CONNECTED;
    conn.mtu = pClient->getMTU() - MTU::ATT_OVERHEAD;

    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(100))) {
        _connections[conn_handle] = conn;
        _clients[conn_handle] = pClient;
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform: onConnect(client): mutex timeout, handle=" +
                std::to_string(conn_handle) + " not tracked");
    }

    DEBUG("NimBLEPlatform: Connected to peripheral: " + peer_addr.toString() +
          " handle=" + std::to_string(conn_handle) + " mtu=" + std::to_string(conn.mtu));

    // Signal async connect completion
    _async_connect_pending = false;
    _async_connect_failed = false;

    // When _native_connect_pending is true, connectNative() is doing a blocking
    // connect and will fire _on_connected itself from the calling task.
    // Firing it here (in the NimBLE host task) would deadlock because _on_connected
    // triggers blocking GATT operations that require the host task to be free.
    if (!_native_connect_pending && _on_connected) {
        _on_connected(conn);
    }
}

void NimBLEPlatform::onConnectFail(NimBLEClient* pClient, int reason) {
    BLEAddress peer_addr = fromNimBLE(pClient->getPeerAddress());
    ERROR("NimBLEPlatform: onConnectFail to " + peer_addr.toString() +
          " reason=" + std::to_string(reason));

    // Signal async connect failure
    _async_connect_pending = false;
    _async_connect_failed = true;
    _async_connect_error = reason;
}

void NimBLEPlatform::onDisconnect(NimBLEClient* pClient, int reason) {
    uint16_t conn_handle = pClient->getConnHandle();

    // During shutdown, cleanup is handled by shutdown() itself.
    // Calling deleteClient here would double-free.
    if (_shutting_down) {
        DEBUG("NimBLEPlatform: onDisconnect during shutdown, skipping cleanup for handle " +
              std::to_string(conn_handle));
        return;
    }

    DEBUG("NimBLEPlatform: Client disconnect event for handle=" + std::to_string(conn_handle) +
          " reason=" + std::to_string(reason));

    // Defer map cleanup to BLE loop task to avoid data race.
    // This callback runs in the NimBLE host task while the BLE loop task
    // may be iterating _connections/_clients concurrently.
    // Note: NimBLEDevice::deleteClient() for this client will be called
    // in processPendingDisconnects() from the loop task context.
    queueDisconnect(conn_handle, reason, false);
}

//=============================================================================
// NimBLE Scan Callbacks
//=============================================================================

void NimBLEPlatform::onResult(const NimBLEAdvertisedDevice* advertisedDevice) {
    // Check if device has our service UUID
    bool hasService = advertisedDevice->isAdvertisingService(BLEUUID(UUID::SERVICE));

    // Debug: log RNS device scan results with address type
    if (hasService) {
        INFO("BLE SCAN: RNS device found: " + std::string(advertisedDevice->getAddress().toString().c_str()) +
             " type=" + std::to_string(advertisedDevice->getAddress().getType()) +
             " RSSI=" + std::to_string(advertisedDevice->getRSSI()) +
             " name=" + advertisedDevice->getName());

        // Cache the full device info for later connection
        // Using string key since NimBLEAdvertisedDevice stores all connection metadata
        std::string addrKey = advertisedDevice->getAddress().toString().c_str();

        // Bounded cache with connected device protection (CONC-M6)
        static constexpr size_t MAX_DISCOVERED_DEVICES = 16;
        while (_discovered_devices.size() >= MAX_DISCOVERED_DEVICES) {
            bool evicted = false;
            // Find oldest non-connected device using insertion order
            for (auto it = _discovered_order.begin(); it != _discovered_order.end(); ++it) {
                if (!isDeviceConnected(*it)) {
                    _discovered_devices.erase(*it);
                    _discovered_order.erase(it);
                    evicted = true;
                    break;
                }
            }
            if (!evicted) {
                // All cached devices are connected - don't cache new one
                WARNING("NimBLEPlatform: Cannot cache device - all slots hold connected devices");
                return;
            }
        }

        // Track insertion order for new devices
        auto existing = _discovered_devices.find(addrKey);
        if (existing == _discovered_devices.end()) {
            // New device - add to order tracking
            _discovered_order.push_back(addrKey);
        }
        _discovered_devices[addrKey] = *advertisedDevice;
        TRACE("NimBLEPlatform: Cached device for connection: " + addrKey +
              " (cache size: " + std::to_string(_discovered_devices.size()) + ")");
    }

    if (hasService && _on_scan_result) {
        ScanResult result;
        result.address = fromNimBLE(advertisedDevice->getAddress());
        result.name = advertisedDevice->getName();
        result.rssi = advertisedDevice->getRSSI();
        result.connectable = advertisedDevice->isConnectable();
        result.has_reticulum_service = true;

        // Extract identity prefix from device name (Protocol v2.2)
        // Format: "RNS-xxxxxx" where xxxxxx is 6 hex chars (3 bytes of identity)
        std::string name = advertisedDevice->getName();
        if (name.size() >= 10 && name.substr(0, 4) == "RNS-") {
            std::string hexPart = name.substr(4, 6);
            if (hexPart.size() == 6) {
                // Parse hex to bytes
                uint8_t prefix[3];
                bool valid = true;
                for (int i = 0; i < 3 && valid; i++) {
                    unsigned int val;
                    if (sscanf(hexPart.c_str() + i*2, "%02x", &val) == 1) {
                        prefix[i] = static_cast<uint8_t>(val);
                    } else {
                        valid = false;
                    }
                }
                if (valid) {
                    result.identity_prefix = Bytes(prefix, 3);
                    DEBUG("NimBLEPlatform: Extracted identity prefix from name: " + hexPart);
                }
            }
        }

        _on_scan_result(result);
    }
}

void NimBLEPlatform::onScanEnd(const NimBLEScanResults& results, int reason) {
    // Check if we were actively scanning
    portENTER_CRITICAL(&_state_mux);
    MasterState prev_master = _master_state;
    bool was_scanning = (prev_master == MasterState::SCANNING ||
                         prev_master == MasterState::SCAN_STARTING ||
                         prev_master == MasterState::SCAN_STOPPING);
    // Transition to IDLE
    if (was_scanning) {
        _master_state = MasterState::IDLE;
        _gap_state = GAPState::READY;
    }
    portEXIT_CRITICAL(&_state_mux);

    _scan_stop_time = 0;

    INFO("BLE SCAN: Ended, reason=" + std::to_string(reason) +
         " found=" + std::to_string(results.getCount()) + " devices");

    // Only process if we were actively scanning (not a spurious callback)
    if (!was_scanning) {
        return;
    }

    // Resume slave if it was paused for this scan
    resumeSlave();

    if (_on_scan_complete) {
        _on_scan_complete();
    }
}

//=============================================================================
// BLEOperationQueue Implementation
//=============================================================================

bool NimBLEPlatform::executeOperation(const GATTOperation& op) {
    // Most operations are executed directly in NimBLE
    // This is a placeholder for more complex queued operations
    return true;
}

//=============================================================================
// Private Methods
//=============================================================================

bool NimBLEPlatform::setupServer() {
    _server = NimBLEDevice::createServer();
    if (!_server) {
        ERROR("NimBLEPlatform: Failed to create server");
        return false;
    }

    _server->setCallbacks(this);

    // Create Reticulum service
    _service = _server->createService(UUID::SERVICE);
    if (!_service) {
        ERROR("NimBLEPlatform: Failed to create service");
        return false;
    }

    // Create RX characteristic (write from central)
    _rx_char = _service->createCharacteristic(
        UUID::RX_CHAR,
        NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR
    );
    _rx_char->setValue((uint8_t*)"\x00", 1);  // Initialize to 0x00
    _rx_char->setCallbacks(this);

    // Create TX characteristic (notify/indicate to central)
    // Note: indicate property required for compatibility with ble-reticulum/Columba
    _tx_char = _service->createCharacteristic(
        UUID::TX_CHAR,
        NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::INDICATE
    );
    _tx_char->setValue((uint8_t*)"\x00", 1);  // Initialize to 0x00 (matches Columba)
    _tx_char->setCallbacks(this);

    // Create Identity characteristic (read only)
    _identity_char = _service->createCharacteristic(
        UUID::IDENTITY_CHAR,
        NIMBLE_PROPERTY::READ
    );
    _identity_char->setCallbacks(this);

    // Start service
    _service->start();

    return setupAdvertising();
}

bool NimBLEPlatform::setupAdvertising() {
    _advertising_obj = NimBLEDevice::getAdvertising();
    if (!_advertising_obj) {
        ERROR("NimBLEPlatform: Failed to get advertising");
        return false;
    }

    // CRITICAL: Reset advertising state before configuring
    // Without this, the advertising data may not be properly updated on ESP32-S3
    _advertising_obj->reset();

    _advertising_obj->setMinInterval(_config.adv_interval_min_ms * 1000 / 625);  // Convert to 0.625ms units
    _advertising_obj->setMaxInterval(_config.adv_interval_max_ms * 1000 / 625);

    // NimBLE 2.x: Use addServiceUUID to include service in advertising packet
    // The name goes in scan response automatically when enableScanResponse is used
    _advertising_obj->addServiceUUID(NimBLEUUID(UUID::SERVICE));
    _advertising_obj->setName(_config.device_name);

    DEBUG("NimBLEPlatform: Advertising configured with service UUID: " + std::string(UUID::SERVICE));

    return true;
}

bool NimBLEPlatform::setupScan() {
    _scan = NimBLEDevice::getScan();
    if (!_scan) {
        ERROR("NimBLEPlatform: Failed to get scan");
        return false;
    }

    _scan->setScanCallbacks(this, false);
    _scan->setActiveScan(_config.scan_mode == ScanMode::ACTIVE);
    _scan->setInterval(_config.scan_interval_ms);
    _scan->setWindow(_config.scan_window_ms);
    _scan->setFilterPolicy(BLE_HCI_SCAN_FILT_NO_WL);
    _scan->setDuplicateFilter(true);  // Filter duplicates within a scan window
    // Don't call setMaxResults - let NimBLE use defaults

    DEBUG("NimBLEPlatform: Scan configured - interval=" + std::to_string(_config.scan_interval_ms) +
          " window=" + std::to_string(_config.scan_window_ms));

    return true;
}

BLEAddress NimBLEPlatform::fromNimBLE(const NimBLEAddress& addr) {
    BLEAddress result;
    const ble_addr_t* base = addr.getBase();
    if (base) {
        // NimBLE stores addresses in little-endian: val[0]=LSB, val[5]=MSB
        // Our BLEAddress stores in big-endian display order: addr[0]=MSB, addr[5]=LSB
        // Need to reverse the byte order
        for (int i = 0; i < 6; i++) {
            result.addr[i] = base->val[5 - i];
        }
    }
    result.type = addr.getType();
    return result;
}

NimBLEAddress NimBLEPlatform::toNimBLE(const BLEAddress& addr) {
    // Use NimBLEAddress string constructor - it parses "XX:XX:XX:XX:XX:XX" format
    // and handles the byte order internally
    std::string addrStr = addr.toString();
    NimBLEAddress nimAddr(addrStr.c_str(), addr.type);
    DEBUG("NimBLEPlatform::toNimBLE: input=" + addrStr +
          " type=" + std::to_string(addr.type) +
          " -> nimAddr=" + std::string(nimAddr.toString().c_str()) +
          " nimType=" + std::to_string(nimAddr.getType()));
    return nimAddr;
}

NimBLEClient* NimBLEPlatform::findClient(uint16_t conn_handle) {
    auto it = _clients.find(conn_handle);
    return (it != _clients.end()) ? it->second : nullptr;
}

NimBLEClient* NimBLEPlatform::findClient(const BLEAddress& address) {
    for (const auto& kv : _clients) {
        if (kv.second && fromNimBLE(kv.second->getPeerAddress()) == address) {
            return kv.second;
        }
    }
    return nullptr;
}

uint16_t NimBLEPlatform::allocateConnHandle() {
    return _next_conn_handle++;
}

void NimBLEPlatform::freeConnHandle(uint16_t handle) {
    // No-op for simple allocator
}

void NimBLEPlatform::updateConnectionMTU(uint16_t conn_handle, uint16_t mtu) {
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(50))) {
        auto it = _connections.find(conn_handle);
        if (it != _connections.end()) {
            it->second.mtu = mtu - MTU::ATT_OVERHEAD;
        }
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform::updateConnectionMTU: mutex timeout for handle " +
                std::to_string(conn_handle));
    }
}

}} // namespace RNS::BLE

#endif // ESP32 && USE_NIMBLE