/**
 * @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 <algorithm>

// 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);
}

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;
    }

    // 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();
        }
    }

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

void NimBLEPlatform::shutdown() {
    INFO("NimBLEPlatform: Beginning graceful shutdown");

    // 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();

    // Disconnect and cleanup clients with mutex protection
    if (xSemaphoreTake(_conn_mutex, pdMS_TO_TICKS(1000))) {
        for (auto& kv : _clients) {
            if (kv.second) {
                NimBLEDevice::deleteClient(kv.second);
            }
        }
        _clients.clear();
        _connections.clear();
        _discovered_devices.clear();
        _discovered_order.clear();
        xSemaphoreGive(_conn_mutex);
    } else {
        WARNING("NimBLEPlatform: Could not acquire mutex for cleanup - forcing cleanup");
        // Force cleanup anyway to prevent leaks
        for (auto& kv : _clients) {
            if (kv.second) {
                NimBLEDevice::deleteClient(kv.second);
            }
        }
        _clients.clear();
        _connections.clear();
        _discovered_devices.clear();
        _discovered_order.clear();
    }

    // Deinit NimBLE stack
    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;

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

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

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

bool NimBLEPlatform::recoverBLEStack() {
    // CONC-M4: Enhanced soft reset with graceful shutdown
    INFO("NimBLEPlatform: Soft reset requested");

    // Track consecutive recovery attempts using existing member variable
    _lightweight_reset_fails++;
    WARNING("NimBLEPlatform: Performing soft BLE reset (attempt " +
            std::to_string(_lightweight_reset_fails) + ")...");

    // If we've had too many consecutive recovery attempts without success,
    // the BLE stack is truly stuck. Reboot is the only reliable fix.
    if (_lightweight_reset_fails >= 5) {
        ERROR("NimBLEPlatform: BLE stack unrecoverable after " +
              std::to_string(_lightweight_reset_fails) + " attempts - rebooting device");
        // DELAY RATIONALE: Stack init settling - allow log message to flush before reboot
        delay(100);
        ESP.restart();
        return false;  // Won't reach here
    }

    // CONC-M4: Use graceful shutdown to wait for active operations
    // This ensures write operations complete before we reset state
    // Save config before shutdown clears it
    PlatformConfig saved_config = _config;

    // Perform graceful shutdown (waits for writes, cleans up properly)
    shutdown();

    // Brief delay for NimBLE host task to process shutdown
    // DELAY RATIONALE: Soft reset processing - allow stack to fully quiesce after deinit
    delay(100);

    // Reinitialize with saved config
    if (!initialize(saved_config)) {
        WARNING("NimBLEPlatform: Soft reset reinitialization failed");
        // Log detailed state for debugging
        ERROR("NimBLEPlatform: Soft reset failed - stack may need hard recovery (ESP.restart)");
        // Return false - caller can decide to trigger ESP.restart() if needed
        return false;
    }

    // Restart the platform
    if (!start()) {
        WARNING("NimBLEPlatform: Soft reset start failed");
        return false;
    }

    // Reset failure counters on successful reset
    _lightweight_reset_fails = 0;
    _scan_fail_count = 0;

    INFO("NimBLEPlatform: Soft reset complete");
    return true;
}

//=============================================================================
// 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() {
    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_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
    // This is critical - the host needs time to fully reset and resync with controller
    if (!ble_hs_synced()) {
        DEBUG("NimBLEPlatform: Waiting for host sync during recovery...");
        uint32_t sync_start = millis();
        while (!ble_hs_synced() && (millis() - sync_start) < 3000) {
            // DELAY RATIONALE: Error recovery before retry
            // After BLE operation failure, NimBLE stack needs processing time before retry.
            // Without delay: immediate retry fails, rapid retries can trigger assertions.
            // 50ms = 5 NimBLE scheduler ticks, empirically chosen balance of recovery vs latency.
            delay(50);
        }
        if (ble_hs_synced()) {
            DEBUG("NimBLEPlatform: Host sync restored after " +
                  std::to_string(millis() - sync_start) + "ms");
        } else {
            ERROR("NimBLEPlatform: Host sync failed during recovery");
            // Continue anyway - may recover on next attempt
        }
    }

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

    // 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();
    }
}

//=============================================================================
// 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;
    }

    // Log GAP hardware state before checking
    DEBUG("NimBLEPlatform: Pre-scan GAP state: disc=" + std::to_string(ble_gap_disc_active()) +
          " adv=" + std::to_string(ble_gap_adv_active()) +
          " conn=" + std::to_string(ble_gap_conn_active()));

    // Verify we can start scan
    if (!canStartScan()) {
        DEBUG("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;
        DEBUG("NimBLEPlatform: Scan started, will stop in " + std::to_string(duration_ms) + "ms");
        return true;
    }

    // Scan failed
    ERROR("NimBLEPlatform: Failed to start scan");

    // 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) {
        WARNING("NimBLEPlatform: Too many scan failures, entering error recovery");
        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);

    // 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)
                if (disc_reason == 574) {
                    platform->_conn_establish_fail_count++;
                    WARNING("NimBLEPlatform: Connection establishment failed (574), count=" +
                            std::to_string(platform->_conn_establish_fail_count));

                    // If too many consecutive failures, trigger recovery
                    if (platform->_conn_establish_fail_count >= CONN_ESTABLISH_FAIL_THRESHOLD) {
                        WARNING("NimBLEPlatform: Too many connection establishment failures, entering recovery");
                        platform->_conn_establish_fail_count = 0;
                        platform->enterErrorRecovery();
                    }
                }
            }

            // Clean up established connections (handles MAC rotation, out of range, etc.)
            auto conn_it = platform->_connections.find(disc_handle);
            if (conn_it != platform->_connections.end()) {
                ConnectionHandle conn = conn_it->second;
                platform->_connections.erase(conn_it);

                INFO("NimBLEPlatform: Native connection lost to " + conn.peer_address.toString() +
                     " reason=" + std::to_string(disc_reason));

                // Clean up client object
                auto client_it = platform->_clients.find(disc_handle);
                if (client_it != platform->_clients.end()) {
                    if (client_it->second) {
                        NimBLEDevice::deleteClient(client_it->second);
                    }
                    platform->_clients.erase(client_it);
                }

                // Clear operation queue for this connection
                platform->clearForConnection(disc_handle);

                // Notify higher layers
                if (platform->_on_disconnected) {
                    platform->_on_disconnected(conn, static_cast<uint8_t>(disc_reason));
                }

                // Restart advertising if in peripheral/dual mode and not currently advertising
                if ((platform->_config.role == Role::PERIPHERAL || platform->_config.role == Role::DUAL) &&
                    !platform->isAdvertising()) {
                    platform->startAdvertising();
                }
            }
            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) {
    DEBUG("NimBLEPlatform::connectNative: Starting native connection to " + address.toString());

    // Verify host-controller sync before connection attempt
    if (!ble_hs_synced()) {
        WARNING("NimBLEPlatform::connectNative: Host not synced, waiting for sync");
        uint32_t sync_start = millis();
        while (!ble_hs_synced() && (millis() - sync_start) < 1000) {
            // DELAY RATIONALE: Notification send retry - respect scheduler tick for queue processing
            delay(10);
        }
        if (!ble_hs_synced()) {
            ERROR("NimBLEPlatform::connectNative: Host sync timeout, entering error recovery");
            enterErrorRecovery();
            return false;
        }
        DEBUG("NimBLEPlatform::connectNative: Host sync restored");
    }

    // Validate address type
    // BLE_ADDR_PUBLIC = 0, BLE_ADDR_RANDOM = 1, BLE_ADDR_PUBLIC_ID = 2, BLE_ADDR_RANDOM_ID = 3
    if (address.type > 3) {
        ERROR("NimBLEPlatform::connectNative: Invalid address type " + std::to_string(address.type));
        return false;
    }

    DEBUG("NimBLEPlatform::connectNative: Connecting to " + address.toString() +
          " type=" + std::to_string(address.type) +
          (address.type == 0 ? " (public)" : address.type == 1 ? " (random)" : " (other)"));

    // Build the peer address structure
    ble_addr_t peer_addr;
    peer_addr.type = address.type;
    // 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
    for (int i = 0; i < 6; i++) {
        peer_addr.val[i] = address.addr[5 - i];
    }

    DEBUG("NimBLEPlatform::connectNative: peer_addr type=" + std::to_string(peer_addr.type) +
          " val=" + std::to_string(peer_addr.val[5]) + ":" +
          std::to_string(peer_addr.val[4]) + ":" +
          std::to_string(peer_addr.val[3]) + ":" +
          std::to_string(peer_addr.val[2]) + ":" +
          std::to_string(peer_addr.val[1]) + ":" +
          std::to_string(peer_addr.val[0]));

    // Connection parameters - use reasonable defaults
    struct ble_gap_conn_params conn_params;
    memset(&conn_params, 0, sizeof(conn_params));
    conn_params.scan_itvl = 16;           // 10ms in 0.625ms units
    conn_params.scan_window = 16;         // 10ms in 0.625ms units
    conn_params.itvl_min = 24;            // 30ms in 1.25ms units
    conn_params.itvl_max = 40;            // 50ms in 1.25ms units
    conn_params.latency = 0;
    conn_params.supervision_timeout = 256; // 2560ms in 10ms units
    conn_params.min_ce_len = 0;
    conn_params.max_ce_len = 0;

    // Reset connection state
    _native_connect_pending = true;
    _native_connect_success = false;
    _native_connect_result = 0;
    _native_connect_handle = 0;
    _native_connect_address = address;

    // Use RANDOM own address type (same as what NimBLEDevice is configured with)
    uint8_t own_addr_type = BLE_OWN_ADDR_RANDOM;

    DEBUG("NimBLEPlatform::connectNative: Trying with own_addr_type=" + std::to_string(own_addr_type));

    int rc = ble_gap_connect(own_addr_type,
                              &peer_addr,
                              timeout_ms,
                              &conn_params,
                              nativeGapEventHandler,
                              this);

    DEBUG("NimBLEPlatform::connectNative: ble_gap_connect returned " + std::to_string(rc));

    if (rc != 0) {
        ERROR("NimBLEPlatform::connectNative: ble_gap_connect failed with rc=" + std::to_string(rc));
        _native_connect_pending = false;

        // Special handling for host desync (rc=22 / BLE_HS_ENOTSYNCED)
        if (rc == 22) {  // BLE_HS_ENOTSYNCED
            ERROR("NimBLEPlatform::connectNative: Host desync detected (rc=22), scheduling host reset");
            // Schedule a host reset to resynchronize with controller
            ble_hs_sched_reset(0);
            // DELAY RATIONALE: Soft reset processing - allow stack to process host reset
            delay(50);
            enterErrorRecovery();
        }

        return false;
    }

    // Wait for connection to complete
    unsigned long start = millis();
    while (_native_connect_pending && (millis() - start) < timeout_ms) {
        // DELAY RATIONALE: Reset wait polling - check connection completion per scheduler tick
        delay(10);
    }

    if (_native_connect_pending) {
        // Timeout - try to cancel but only if connection is still pending at GAP level
        WARNING("NimBLEPlatform::connectNative: Connection timed out, cancelling");
        // Check if we're actually connecting before cancelling
        if (ble_gap_conn_active()) {
            int rc = ble_gap_conn_cancel();
            if (rc != 0 && rc != BLE_HS_EALREADY) {
                DEBUG("NimBLEPlatform::connectNative: ble_gap_conn_cancel returned " + std::to_string(rc));
            }
        }
        // DELAY RATIONALE: Stack stabilization after connection cancel
        delay(10);
        _native_connect_pending = false;
        return false;
    }

    if (!_native_connect_success) {
        ERROR("NimBLEPlatform::connectNative: Connection failed with result=" +
              std::to_string(_native_connect_result));
        return false;
    }

    // Copy volatile to local variable
    uint16_t conn_handle = _native_connect_handle;

    INFO("NimBLEPlatform::connectNative: Connection succeeded! handle=" +
         std::to_string(conn_handle));

    // Now create an NimBLEClient for this connection to use for GATT operations
    // The connection already exists, so we just need to wrap it
    NimBLEClient* client = NimBLEDevice::getClientByHandle(conn_handle);
    if (!client) {
        // Create a new client and associate it with this connection
        NimBLEAddress nimAddr(peer_addr);
        client = NimBLEDevice::createClient(nimAddr);
        if (client) {
            client->setClientCallbacks(this, false);
        }
    }

    if (client) {
        // Track the connection
        ConnectionHandle conn;
        conn.handle = conn_handle;
        conn.peer_address = address;
        conn.local_role = Role::CENTRAL;
        conn.state = ConnectionState::CONNECTED;
        conn.mtu = client->getMTU();

        _connections[conn_handle] = conn;
        _clients[conn_handle] = client;

        if (_on_connected) {
            _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) {
    auto client_it = _clients.find(conn_handle);
    if (client_it == _clients.end() || !client_it->second) {
        return false;
    }

    NimBLEClient* client = client_it->second;

    // Get our service
    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
    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
    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;
    }

    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;
    }

    // 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) {
    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 - write to peripheral's RX characteristic
        auto client_it = _clients.find(conn_handle);
        if (client_it == _clients.end() || !client_it->second) {
            return false;
        }

        NimBLEClient* client = client_it->second;
        NimBLERemoteService* service = client->getService(UUID::SERVICE);
        if (!service) return false;

        NimBLERemoteCharacteristic* rxChar = service->getCharacteristic(UUID::RX_CHAR);
        if (!rxChar) return false;

        // CONC-H4: Track active write for graceful shutdown
        beginWriteOperation();
        bool result = rxChar->writeValue(data.data(), data.size(), response);
        endWriteOperation();
        return result;
    } else {
        // We are peripheral - this shouldn't be used, use notify instead
        WARNING("NimBLEPlatform: write() called in peripheral mode, use notify()");
        return false;
    }
}

bool NimBLEPlatform::read(uint16_t conn_handle, uint16_t char_handle,
                          std::function<void(OperationResult, const Bytes&)> callback) {
    auto client_it = _clients.find(conn_handle);
    if (client_it == _clients.end() || !client_it->second) {
        if (callback) callback(OperationResult::NOT_FOUND, Bytes());
        return false;
    }

    NimBLEClient* client = client_it->second;
    NimBLERemoteService* service = client->getService(UUID::SERVICE);
    if (!service) {
        if (callback) callback(OperationResult::NOT_FOUND, Bytes());
        return false;
    }

    // Find characteristic by handle
    NimBLERemoteCharacteristic* chr = nullptr;
    if (char_handle == _connections[conn_handle].identity_handle) {
        chr = service->getCharacteristic(UUID::IDENTITY_CHAR);
    }

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

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

    return true;
}

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

    NimBLEClient* client = client_it->second;
    NimBLERemoteService* service = client->getService(UUID::SERVICE);
    if (!service) return false;

    NimBLERemoteCharacteristic* txChar = service->getCharacteristic(UUID::TX_CHAR);
    if (!txChar) return false;

    if (enable) {
        // Subscribe to notifications
        auto notifyCb = [this, conn_handle](NimBLERemoteCharacteristic* pChar,
                                             uint8_t* pData, size_t length, bool isNotify) {
            if (_on_data_received) {
                ConnectionHandle conn = getConnection(conn_handle);
                Bytes data(pData, length);
                _on_data_received(conn, data);
            }
        };

        return txChar->subscribe(true, notifyCb);
    } else {
        return txChar->unsubscribe();
    }
}

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

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

bool NimBLEPlatform::notifyAll(const Bytes& data) {
    if (!_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;
    for (const auto& kv : _connections) {
        result.push_back(kv.second);
    }
    return result;
}

ConnectionHandle NimBLEPlatform::getConnection(uint16_t handle) const {
    auto it = _connections.find(handle);
    if (it != _connections.end()) {
        return it->second;
    }
    return ConnectionHandle();
}

size_t NimBLEPlatform::getConnectionCount() const {
    return _connections.size();
}

bool NimBLEPlatform::isConnectedTo(const BLEAddress& address) const {
    for (const auto& kv : _connections) {
        if (kv.second.peer_address == address) {
            return true;
        }
    }
    return false;
}

bool NimBLEPlatform::isDeviceConnected(const std::string& addrKey) const {
    for (const auto& [handle, conn] : _connections) {
        if (conn.peer_address.toString() == addrKey) {
            return true;
        }
    }
    return false;
}

//=============================================================================
// 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 {
    return fromNimBLE(NimBLEDevice::getAddress());
}

//=============================================================================
// 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;

    _connections[conn_handle] = conn;

    DEBUG("NimBLEPlatform: Central connected: " + conn.peer_address.toString());

    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) {
    uint16_t conn_handle = connInfo.getConnHandle();

    auto it = _connections.find(conn_handle);
    if (it != _connections.end()) {
        ConnectionHandle conn = it->second;
        _connections.erase(it);

        DEBUG("NimBLEPlatform: Central disconnected: " + conn.peer_address.toString() +
              " reason: " + std::to_string(reason));

        if (_on_central_disconnected) {
            _on_central_disconnected(conn);
        }
    }

    // Clear operation queue for this connection
    BLEOperationQueue::clearForConnection(conn_handle);
}

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();

    _connections[conn_handle] = conn;
    _clients[conn_handle] = pClient;

    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;

    if (_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();

    auto it = _connections.find(conn_handle);
    if (it != _connections.end()) {
        ConnectionHandle conn = it->second;
        _connections.erase(it);

        DEBUG("NimBLEPlatform: Disconnected from peripheral: " + conn.peer_address.toString() +
              " reason: " + std::to_string(reason));

        if (_on_disconnected) {
            _on_disconnected(conn, static_cast<uint8_t>(reason));
        }
    }

    // Remove client
    _clients.erase(conn_handle);
    NimBLEDevice::deleteClient(pClient);

    // Clear operation queue
    BLEOperationQueue::clearForConnection(conn_handle);
}

//=============================================================================
// 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) {
        DEBUG("NimBLEPlatform: 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;

    DEBUG("NimBLEPlatform: onScanEnd callback, reason=" + std::to_string(reason) +
          " found=" + std::to_string(results.getCount()) + " devices" +
          " was_scanning=" + std::string(was_scanning ? "yes" : "no"));

    // 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) {
    auto it = _connections.find(conn_handle);
    if (it != _connections.end()) {
        it->second.mtu = mtu;
    }
}

}} // namespace RNS::BLE

#endif // ESP32 && USE_NIMBLE