BluetoothController.cpp

Go to the documentation of this file.

 
#include "cdc_hal/IBluetoothController.h"
#include "cdc_hal/hw_config.h"
#include "cdc_core/Raii.h"
#include "cdc_log.h"
#include "sdkconfig.h"
#include "esp_attr.h"
 
static const char* TAG = "BT-Ctrl";
 
// Full implementation is compiled only when NimBLE support is enabled.
#if defined(CONFIG_BT_ENABLED) && defined(CONFIG_BT_NIMBLE_ENABLED)
 
#include "esp_bt.h"
#include "esp_mac.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "host/ble_hs.h"
#include "host/util/util.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"
#include "host/ble_uuid.h"
#include "host/ble_att.h"
#include "host/util/util.h"
#include "host/ble_store.h"
#include "store/config/ble_store_config.h"

extern "C" void ble_store_config_init(void);
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include <cstring>
#include <algorithm>
 
namespace cdc::hal {

static void bleHostTask(void* param);

static int gattcSvcDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                           const struct ble_gatt_svc* service, void* arg);
static int gattcChrDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                           const struct ble_gatt_chr* chr, void* arg);
static int gattcReadCb(uint16_t connHandle, const struct ble_gatt_error* error,
                        struct ble_gatt_attr* attr, void* arg);
static int gattcWriteCb(uint16_t connHandle, const struct ble_gatt_error* error,
                          struct ble_gatt_attr* attr, void* arg);
static int gattcDscDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                           uint16_t chr_val_handle, const struct ble_gatt_dsc* dsc, void* arg);

 
static constexpr uint8_t MAX_REGISTERED_SERVICES = IBluetoothController::MAX_REGISTERED_SERVICES;
static constexpr uint8_t PLUGIN_SERVICE_SLOT = MAX_REGISTERED_SERVICES - 1;
static constexpr uint8_t MAX_CHARS_PER_SERVICE = IBluetoothController::MAX_CHARS_PER_SERVICE;
static constexpr uint8_t MAX_DESCRIPTORS_PER_CHAR = 2;
static constexpr uint8_t MAX_ADV_UUIDS = 4;
static constexpr uint8_t MAX_CONN_CALLBACKS = 6;
static constexpr uint8_t MAX_CONNECTIONS = 2;
static constexpr uint8_t MAX_SUBSCRIBE_ENTRIES = 16;
static constexpr uint8_t MAX_BONDS = 5;

static constexpr uint32_t kConnectSettleMs = 50;
static constexpr uint32_t kDisconnectDrainPollMs = 20;
static constexpr uint32_t kDisconnectDrainTimeoutMs = 1000;

struct InternalService {
    bool active = false;
 
    // NimBLE-native UUID storage
    ble_uuid_any_t svcUuid;
    ble_uuid_any_t charUuids[MAX_CHARS_PER_SERVICE];
 
    // NimBLE characteristic + service definitions (must persist)
    ble_gatt_chr_def nimbleChars[MAX_CHARS_PER_SERVICE + 1]; // +1 terminator
    ble_gatt_svc_def nimbleSvcs[2];                          // +1 terminator
 
    // Per-characteristic descriptor backing storage (e.g. HID Report Reference).
    // Packed layout per descriptor slot: [0] = data length, [1..4] = data bytes.
    ble_uuid_any_t dscUuids[MAX_CHARS_PER_SERVICE][MAX_DESCRIPTORS_PER_CHAR];
    ble_gatt_dsc_def dscDefs[MAX_CHARS_PER_SERVICE][MAX_DESCRIPTORS_PER_CHAR + 1];
    uint8_t dscPacked[MAX_CHARS_PER_SERVICE][MAX_DESCRIPTORS_PER_CHAR][5];
 
    // Callbacks registered by the module
    GattWriteCallback writeCallbacks[MAX_CHARS_PER_SERVICE];
    GattReadCallback readCallbacks[MAX_CHARS_PER_SERVICE];
    uint8_t numChars = 0;
};
 
EXT_RAM_BSS_ATTR static InternalService s_services[MAX_REGISTERED_SERVICES];

static void convertUuid(const BleUuid& src, ble_uuid_any_t& dst) {
    if (src.type == BleUuid::UUID_16) {
        dst.u.type = BLE_UUID_TYPE_16;
        dst.u16.u.type = BLE_UUID_TYPE_16;
        dst.u16.value = src.u16;
    } else {
        dst.u.type = BLE_UUID_TYPE_128;
        dst.u128.u.type = BLE_UUID_TYPE_128;
        memcpy(dst.u128.value, src.u128, 16);
    }
}

static ble_gatt_chr_flags mapProperties(uint8_t props, uint8_t perms) {
    ble_gatt_chr_flags flags = 0;
    if (props & GattProp::READ)         flags |= BLE_GATT_CHR_F_READ;
    if (props & GattProp::WRITE)        flags |= BLE_GATT_CHR_F_WRITE;
    if (props & GattProp::WRITE_NO_RSP) flags |= BLE_GATT_CHR_F_WRITE_NO_RSP;
    if (props & GattProp::NOTIFY)       flags |= BLE_GATT_CHR_F_NOTIFY;
    if (props & GattProp::INDICATE)     flags |= BLE_GATT_CHR_F_INDICATE;
 
    // Encryption requirements
    if (perms & GattPerm::READ_ENC)     flags |= BLE_GATT_CHR_F_READ_ENC;
    if (perms & GattPerm::WRITE_ENC)    flags |= BLE_GATT_CHR_F_WRITE_ENC;
 
    return flags;
}

EXT_RAM_BSS_ATTR static uint8_t s_gattAccessBuf[512];
 
static int gattServiceAccessCb(uint16_t connHandle, uint16_t attrHandle,
                                struct ble_gatt_access_ctxt* ctxt, void* arg) {
    auto* svc = static_cast<InternalService*>(arg);
    if (!svc) return BLE_ATT_ERR_UNLIKELY;
 
    // Find which characteristic was accessed by matching UUID
    for (uint8_t i = 0; i < svc->numChars; i++) {
        if (ble_uuid_cmp(ctxt->chr->uuid, &svc->charUuids[i].u) == 0) {
            if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR && svc->writeCallbacks[i]) {
                uint16_t len = OS_MBUF_PKTLEN(ctxt->om);
                if (len > sizeof(s_gattAccessBuf)) len = sizeof(s_gattAccessBuf);
                ble_hs_mbuf_to_flat(ctxt->om, s_gattAccessBuf, len, nullptr);
                return svc->writeCallbacks[i](connHandle, attrHandle, s_gattAccessBuf, len);
            }
            if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR && svc->readCallbacks[i]) {
                uint16_t len = sizeof(s_gattAccessBuf);
                int rc = svc->readCallbacks[i](connHandle, attrHandle, s_gattAccessBuf, &len);
                if (rc == 0 && len > 0) {
                    os_mbuf_append(ctxt->om, s_gattAccessBuf, len);
                }
                return rc;
            }
            return 0; // No callback = allow silently
        }
    }
 
    return BLE_ATT_ERR_UNLIKELY;
}

static int gattStaticDescriptorAccessCb(uint16_t connHandle, uint16_t attrHandle,
                                          struct ble_gatt_access_ctxt* ctxt, void* arg) {
    (void)connHandle;
    (void)attrHandle;
    if (ctxt->op != BLE_GATT_ACCESS_OP_READ_DSC) return BLE_ATT_ERR_UNLIKELY;
    if (!arg) return BLE_ATT_ERR_UNLIKELY;
 
    // Layout: [0] = length, [1..4] = bytes
    const uint8_t* packed = static_cast<const uint8_t*>(arg);
    uint8_t len = packed[0];
    return os_mbuf_append(ctxt->om, packed + 1, len) == 0 ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES;
}

class BluetoothController : public IBluetoothController {
public:
    BluetoothController() {
        instance_ = this;
        lifecycleMutex_ = xSemaphoreCreateRecursiveMutex();
    }

    bool init() override;
    bool start() override;
    void stop() override;
    core::ServiceState getState() const override { return state_; }
    const char* getName() const override { return "bluetooth"; }

    bool enable() override;
    void disable() override;
    void notifySystemReady() override;
    bool isEnabled() const override { return enabled_; }
    bool getMacAddress(uint8_t* mac) const override;
    void setDeviceName(const char* name) override;
    const char* getDeviceName() const override { return deviceName_; }
    bool isConnected() const override;
    void disconnect() override;
    int8_t getRssi() const override;

    void startAdvertising() override;
    void stopAdvertising() override;
    bool isAdvertising() const override { return ble_gap_adv_active() != 0; }
    bool addAdvertisingUuid(const BleUuid& uuid) override;
    void removeAdvertisingUuid(const BleUuid& uuid) override;
    bool setAdvertisingManufacturerData(uint16_t companyId,
                                         const uint8_t* data, uint16_t len) override;
    void clearAdvertisingManufacturerData() override;
    void setAppearance(uint16_t appearance) override;

    bool startScan(uint32_t durationMs, bool keepAdvertising) override;
    void stopScan() override;
    bool isScanComplete() const override { return !scanning_; }
    uint8_t getScanResults(BleScanResult* results, uint8_t maxResults) override;

    bool registerGattService(const GattServiceDef& service,
                             bool pluginReserved = false) override;
    bool unregisterGattService(const BleUuid& serviceUuid) override;
    bool sendNotification(uint16_t connHandle, uint16_t attrHandle,
                          const uint8_t* data, uint16_t len) override;
    uint16_t getMtu() const override;
    uint16_t getConnectionHandle() const override { return primaryConnHandle(); }
    void clearAllBonds() override;

    bool connect(const uint8_t* addr, uint8_t addrType) override;
    void cancelConnect() override;
    bool discoverServiceByUuid(uint16_t connHandle, const BleUuid& uuid) override;
    bool writeCharacteristic(uint16_t connHandle, uint16_t attrHandle,
                             const uint8_t* data, uint16_t len,
                             bool withResponse) override;
    bool readCharacteristic(uint16_t connHandle, uint16_t attrHandle) override;
    bool enableNotifications(uint16_t connHandle, uint16_t cccdHandle) override;
    bool subscribeToCharacteristic(uint16_t connHandle, uint16_t valueHandle) override;
    void disconnectHandle(uint16_t connHandle) override;
    ListenerToken addServiceDiscoveryCallback(ServiceDiscoveryCallback cb) override;
    ListenerToken addCharacteristicReadCallback(CharacteristicReadCallback cb) override;
    ListenerToken addNotificationCallback(NotificationCallback cb) override;
    ListenerToken addWriteCompleteCallback(WriteCompleteCallback cb) override;
    void removeServiceDiscoveryCallback(ListenerToken token) override;
    void removeCharacteristicReadCallback(ListenerToken token) override;
    void removeNotificationCallback(ListenerToken token) override;
    void removeWriteCompleteCallback(ListenerToken token) override;
    void setServiceDiscoveryCallback(ServiceDiscoveryCallback cb) override;
    void setCharacteristicReadCallback(CharacteristicReadCallback cb) override;
    void setNotificationCallback(NotificationCallback cb) override;
    void setWriteCompleteCallback(WriteCompleteCallback cb) override;

    ListenerToken addConnectionCallback(ConnectionCallback cb) override;
    ListenerToken addDisconnectionCallback(DisconnectionCallback cb) override;
    void removeConnectionCallback(ListenerToken token) override;
    void removeDisconnectionCallback(ListenerToken token) override;

    ListenerToken addNumericComparisonCallback(NumericComparisonCallback cb) override;
    void removeNumericComparisonCallback(ListenerToken token) override;
    void setNumericComparisonCallback(NumericComparisonCallback cb) override;
    void respondToNumericComparison(uint16_t connHandle, bool accept) override;
    void setPasskeyCallback(PasskeyCallback cb) override;
    void setAuthCompleteCallback(AuthCompleteCallback cb) override;
    ListenerToken addEncryptionChangeCallback(EncChangeCallback cb) override;
    void removeEncryptionChangeCallback(ListenerToken token) override;
    bool initiateSecurity(uint16_t connHandle) override;
    bool getPeerIdAddr(uint16_t connHandle, uint8_t addr[6], uint8_t* addrType) const override;
    void forgetBond(const uint8_t addr[6], uint8_t addrType) override;
    uint8_t getBondedDevices(BleBondInfo* out, uint8_t maxCount) const override;

    void onConnect(uint16_t connHandle, bool isPeripheral);
    void onDisconnect(uint16_t connHandle, int reason);
    void onSync();
    void onScanResult(const ble_gap_disc_desc* disc);
    void onScanComplete();
    void onAdvComplete();
    void onPasskeyAction(uint16_t connHandle, const ble_gap_passkey_params* params);
    void onEncChange(uint16_t connHandle, int status);
    void onSubscribe(const struct ble_gap_event* event);
    void onMtuExchange(uint16_t connHandle, uint16_t mtu);

    template <typename CB>
    struct ListenerSlot {
        bool active = false;
        CB callback;
    };

    struct ConnectionState {
        bool active = false;
        uint16_t handle = BLE_HS_CONN_HANDLE_NONE;
        bool isPeripheral = false;   // true = we are peripheral, false = central
        uint16_t mtu = 23;            // ATT MTU including 3-byte header
    };

    struct SubscribeEntry {
        bool active = false;
        uint16_t connHandle;
        uint16_t attrHandle;
        bool notify;
        bool indicate;
    };
 
    uint16_t primaryConnHandle() const;
    int8_t findConnectionSlot(uint16_t handle) const;
 
private:
    bool enableNow();
 
    core::ServiceState state_ = core::ServiceState::UNINITIALIZED;
 
    // Serializes stack lifecycle transitions (enable/disable and the GATT
    // register/unregister rebuild) across caller tasks. NimBLE host-task
    // callbacks never take it, so holding it across the blocking teardown
    // cannot deadlock against the host task. Recursive: register -> disable ->
    // enable re-enter on the same task.
    SemaphoreHandle_t lifecycleMutex_ = nullptr;
 
    bool enabled_ = false;
    bool synced_ = false;
    bool systemReady_ = false;
    bool pendingEnable_ = false;
    bool advertising_ = false;
    bool scanning_ = false;
    bool scanWasAdvertising_ = false;
    char deviceName_[32] = "CDC Badge";
    uint8_t ownAddrType_ = BLE_OWN_ADDR_PUBLIC;

    ConnectionState connections_[MAX_CONNECTIONS] = {};

    SubscribeEntry subscribes_[MAX_SUBSCRIBE_ENTRIES] = {};

    BleScanResult scanResults_[MAX_SCAN_RESULTS] = {};
    // Per-result flag: name came from a Complete Local Name (0x09). Prevents
    // downgrading it to a Shortened name (0x08) on later advertising events.
    bool scanNameComplete_[MAX_SCAN_RESULTS] = {};
    uint8_t scanResultCount_ = 0;

    BleUuid advUuids_[MAX_ADV_UUIDS] = {};
    uint8_t advUuidCount_ = 0;

    uint16_t appearance_ = 0;
 
    ListenerSlot<ConnectionCallback> connCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<DisconnectionCallback> disconnCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<NumericComparisonCallback> numCmpCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<ServiceDiscoveryCallback> svcDiscoveryCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<CharacteristicReadCallback> charReadCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<NotificationCallback> notifyCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<WriteCompleteCallback> writeCompleteCallbacks_[MAX_CONN_CALLBACKS] = {};
    ListenerSlot<EncChangeCallback> encChangeCallbacks_[MAX_CONN_CALLBACKS] = {};

    PasskeyCallback passkeyCb_;
    AuthCompleteCallback authCompleteCb_;

    DiscoveredService discoveredSvc_ = {};
    BleUuid discoverTargetUuid_ = {};
    uint16_t discoverSvcStart_ = 0;
    uint16_t discoverSvcEnd_ = 0;

    uint16_t pendingSubValueHandle_ = 0;
    uint16_t pendingSubCccdHandle_ = 0;
 
    // Manufacturer data for advertising
    uint8_t mfgData_[31] = {};
    uint16_t mfgDataLen_ = 0;
    uint16_t mfgCompanyId_ = 0;
    bool mfgDataSet_ = false;
 
    // Singleton access for callbacks
public:
    static BluetoothController* instance_;
    friend void bleHostTask(void* param);
    friend int bleGapEventCallback(struct ble_gap_event* event, void* arg);
    friend int gattcSvcDiscCb(uint16_t, const struct ble_gatt_error*,
                               const struct ble_gatt_svc*, void*);
    friend int gattcChrDiscCb(uint16_t, const struct ble_gatt_error*,
                               const struct ble_gatt_chr*, void*);
    friend int gattcReadCb(uint16_t, const struct ble_gatt_error*,
                            struct ble_gatt_attr*, void*);
    friend int gattcWriteCb(uint16_t, const struct ble_gatt_error*,
                              struct ble_gatt_attr*, void*);
    friend int gattcDscDiscCb(uint16_t, const struct ble_gatt_error*,
                               uint16_t, const struct ble_gatt_dsc*, void*);
};

BluetoothController* BluetoothController::instance_ = nullptr;

int bleGapEventCallback(struct ble_gap_event* event, void* arg) {
    (void)arg;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    switch (event->type) {
        case BLE_GAP_EVENT_CONNECT:
            if (event->connect.status == 0) {
                struct ble_gap_conn_desc desc;
                bool isPeripheral = true;
                if (ble_gap_conn_find(event->connect.conn_handle, &desc) == 0) {
                    isPeripheral = (desc.role == BLE_GAP_ROLE_SLAVE);
                }
                ctrl->onConnect(event->connect.conn_handle, isPeripheral);
            } else {
                LOG_W(TAG, "Connection failed, status=%d", event->connect.status);
            }
            break;
 
        case BLE_GAP_EVENT_DISCONNECT:
            ctrl->onDisconnect(event->disconnect.conn.conn_handle,
                              event->disconnect.reason);
            break;
 
        case BLE_GAP_EVENT_CONN_UPDATE:
            LOG_I(TAG, "Connection updated");
            break;
 
        case BLE_GAP_EVENT_ADV_COMPLETE:
            LOG_D(TAG, "Advertising complete");
            ctrl->onAdvComplete();
            break;
 
        case BLE_GAP_EVENT_MTU:
            LOG_I(TAG, "MTU updated: handle=%d value=%d",
                  event->mtu.conn_handle, event->mtu.value);
            ctrl->onMtuExchange(event->mtu.conn_handle, event->mtu.value);
            break;
 
        case BLE_GAP_EVENT_DISC:
            ctrl->onScanResult(&event->disc);
            break;
 
        case BLE_GAP_EVENT_DISC_COMPLETE:
            LOG_D(TAG, "Scan complete");
            ctrl->onScanComplete();
            break;
 
        case BLE_GAP_EVENT_PASSKEY_ACTION:
            ctrl->onPasskeyAction(event->passkey.conn_handle,
                                  &event->passkey.params);
            break;
 
        case BLE_GAP_EVENT_ENC_CHANGE:
            ctrl->onEncChange(event->enc_change.conn_handle,
                              event->enc_change.status);
            break;
 
        case BLE_GAP_EVENT_REPEAT_PAIRING: {
            // Peer is re-pairing: delete the old bond so the new one can replace it.
            struct ble_gap_conn_desc desc;
            if (ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc) == 0) {
                ble_store_util_delete_peer(&desc.peer_id_addr);
            }
            return BLE_GAP_REPEAT_PAIRING_RETRY;
        }
 
        case BLE_GAP_EVENT_SUBSCRIBE:
            ctrl->onSubscribe(event);
            break;
 
        case BLE_GAP_EVENT_NOTIFY_RX:
            if (event->notify_rx.om) {
                uint16_t len = OS_MBUF_PKTLEN(event->notify_rx.om);
                if (len > sizeof(s_gattAccessBuf)) len = sizeof(s_gattAccessBuf);
                ble_hs_mbuf_to_flat(event->notify_rx.om, s_gattAccessBuf, len, nullptr);
                for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
                    if (ctrl->notifyCallbacks_[i].active &&
                        ctrl->notifyCallbacks_[i].callback) {
                        ctrl->notifyCallbacks_[i].callback(
                            event->notify_rx.conn_handle,
                            event->notify_rx.attr_handle,
                            s_gattAccessBuf, len);
                    }
                }
            }
            break;
 
        default:
            break;
    }
 
    return 0;
}

static void bleSyncCallback() {
    if (BluetoothController::instance_) {
        BluetoothController::instance_->onSync();
    }
}

static void bleResetCallback(int reason) {
    LOG_E(TAG, "BLE host reset, reason=%d", reason);
}

static void bleHostTask(void* param) {
    (void)param;
    LOG_I(TAG, "NimBLE host task started");
    nimble_port_run();
    nimble_port_freertos_deinit();
}

bool BluetoothController::init() {
    if (state_ != core::ServiceState::UNINITIALIZED) {
        return state_ == core::ServiceState::INITIALIZED ||
               state_ == core::ServiceState::STARTED;
    }
 
    // instance_ is set in the constructor, but reaffirm here in case multiple
    // instances are ever created (only the most recently initialized wins).
    instance_ = this;
 
    // Release classic BT memory (we only use BLE)
    ESP_ERROR_CHECK(esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT));
 
    state_ = core::ServiceState::INITIALIZED;
    LOG_I(TAG, "Bluetooth controller initialized");
    return true;
}

bool BluetoothController::start() {
    if (state_ == core::ServiceState::INITIALIZED ||
        state_ == core::ServiceState::STOPPED) {
        state_ = core::ServiceState::STARTED;
        return true;
    }
    return state_ == core::ServiceState::STARTED;
}

void BluetoothController::stop() {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    if (state_ == core::ServiceState::STARTED) {
        if (enabled_) {
            disable();
        }
        state_ = core::ServiceState::STOPPED;
    }
}

bool BluetoothController::enable() {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    if (enabled_) {
        return true;
    }
    if (!systemReady_) {
        pendingEnable_ = true;
        LOG_I(TAG, "BLE enable deferred until system ready");
        return true;
    }
    return enableNow();
}

void BluetoothController::notifySystemReady() {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    systemReady_ = true;
    if (pendingEnable_ && !enabled_) {
        pendingEnable_ = false;
        enableNow();
    }
}

bool BluetoothController::enableNow() {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    if (enabled_) {
        return true;
    }
 
    if (state_ != core::ServiceState::STARTED) {
        LOG_E(TAG, "Cannot enable - service not started");
        return false;
    }
 
    // Initialize NimBLE
    esp_err_t ret = nimble_port_init();
    if (ret != ESP_OK) {
        LOG_E(TAG, "nimble_port_init failed: %d", ret);
        return false;
    }
 
    // Configure NimBLE host
    ble_hs_cfg.reset_cb = bleResetCallback;
    ble_hs_cfg.sync_cb = bleSyncCallback;
    ble_hs_cfg.gatts_register_cb = nullptr;
    ble_hs_cfg.store_status_cb = nullptr;
 
    // Security: Display+YesNo for numeric comparison pairing
    ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_DISP_YES_NO;
    ble_hs_cfg.sm_bonding = 1;
    ble_hs_cfg.sm_mitm = 1;
    ble_hs_cfg.sm_sc = 1;
    ble_hs_cfg.sm_our_key_dist = BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID;
    ble_hs_cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID;
 
    // Initialize mandatory GAP and GATT services
    ble_svc_gap_init();
    ble_svc_gatt_init();
 
    // Commit module GATT services registered before BLE was enabled (and restore
    // those from a previous session, since disable() tears down the GATT DB).
    for (int i = 0; i < MAX_REGISTERED_SERVICES; i++) {
        if (!s_services[i].active) continue;
        int grc = ble_gatts_count_cfg(s_services[i].nimbleSvcs);
        if (grc == 0) grc = ble_gatts_add_svcs(s_services[i].nimbleSvcs);
        if (grc != 0) {
            LOG_E(TAG, "Deferred GATT service slot %d commit failed: %d", i, grc);
        } else {
            LOG_I(TAG, "Committed GATT service slot %d", i);
        }
    }
 
    // Wire up the NVS-backed bond store
    ble_store_config_init();
 
    // Set device name
    ble_svc_gap_device_name_set(deviceName_);
 
    // Start NimBLE host task
    nimble_port_freertos_init(bleHostTask);
 
    enabled_ = true;
    LOG_I(TAG, "Bluetooth enabled");
    return true;
}

void BluetoothController::disable() {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    if (!enabled_) {
        return;
    }
 
    // Down before teardown: GAP callbacks dispatched while the host task drains
    // gate advertising restarts on these flags.
    enabled_ = false;
    synced_ = false;
 
    // Stop advertising before touching connections so a bonded peer cannot
    // (re)connect into the teardown window.
    ble_gap_adv_stop();
    advertising_ = false;
 
    // A GAP procedure with a duration timer (scan, pending connection) must be
    // cancelled before nimble_port_deinit() releases the event queue its callout
    // is bound to.
    if (scanning_) {
        ble_gap_disc_cancel();
        scanning_ = false;
        scanWasAdvertising_ = false;
    }
    ble_gap_conn_cancel();
 
    // nimble_port_stop() must not run while a connection is live: in the
    // STOPPING state the host deinits the ble_hs_timer callout while queued
    // timer events (e.g. the 30 s SM timer of an encryption re-establishment)
    // still dereference it - LoadProhibited in npl_freertos_callout_is_active.
    // Let an in-flight connect surface in the connection table, then terminate
    // everything and wait until the host task has processed every disconnect.
    vTaskDelay(pdMS_TO_TICKS(kConnectSettleMs));
    uint32_t waited = 0;
    for (;; waited += kDisconnectDrainPollMs) {
        bool anyActive = false;
        for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) {
            if (connections_[i].active) {
                ble_gap_terminate(connections_[i].handle, BLE_ERR_REM_USER_CONN_TERM);
                anyActive = true;
            }
        }
        if (!anyActive) break;
        if (waited >= kDisconnectDrainTimeoutMs) {
            LOG_W(TAG, "Disconnect drain timed out, forcing BLE shutdown");
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(kDisconnectDrainPollMs));
    }
 
    // Shutdown NimBLE and wait for the host task to actually exit before deinit
    int rc = nimble_port_stop();
    if (rc == 0) {
        // nimble_port_stop signals the host task; give it time to drain
        vTaskDelay(pdMS_TO_TICKS(50));
        nimble_port_deinit();
    }
 
    for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) connections_[i].active = false;
    for (uint8_t i = 0; i < MAX_SUBSCRIBE_ENTRIES; i++) subscribes_[i].active = false;
 
    LOG_I(TAG, "Bluetooth disabled");
}

bool BluetoothController::getMacAddress(uint8_t* mac) const {
    if (!mac) return false;
 
    if (enabled_ && synced_) {
        // Get address from NimBLE
        int rc = ble_hs_id_copy_addr(ownAddrType_, mac, nullptr);
        return rc == 0;
    }
 
    // Fallback: read from efuse
    esp_read_mac(mac, ESP_MAC_BT);
    return true;
}

void BluetoothController::setDeviceName(const char* name) {
    if (!name) return;
 
    strncpy(deviceName_, name, sizeof(deviceName_) - 1);
    deviceName_[sizeof(deviceName_) - 1] = '\0';
 
    if (enabled_ && synced_) {
        ble_svc_gap_device_name_set(deviceName_);
    }
}

void BluetoothController::disconnect() {
    for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) {
        if (connections_[i].active) {
            ble_gap_terminate(connections_[i].handle, BLE_ERR_REM_USER_CONN_TERM);
        }
    }
}
 
bool BluetoothController::isConnected() const {
    for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) {
        if (connections_[i].active) return true;
    }
    return false;
}
 
uint16_t BluetoothController::primaryConnHandle() const {
    for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) {
        if (connections_[i].active) return connections_[i].handle;
    }
    return BLE_HS_CONN_HANDLE_NONE;
}
 
int8_t BluetoothController::findConnectionSlot(uint16_t handle) const {
    for (int8_t i = 0; i < (int8_t)MAX_CONNECTIONS; i++) {
        if (connections_[i].active && connections_[i].handle == handle) return i;
    }
    return -1;
}
 
void BluetoothController::clearAllBonds() {
    int rc = ble_store_clear();
    if (rc != 0) {
        LOG_E(TAG, "ble_store_clear failed: %d", rc);
    } else {
        LOG_I(TAG, "All bonds cleared");
    }
}
 
void BluetoothController::onEncChange(uint16_t connHandle, int status) {
    LOG_I(TAG, "Encryption %s on handle %d (status=%d)",
          status == 0 ? "established" : "failed", connHandle, status);
    if (authCompleteCb_) {
        authCompleteCb_(status == 0);
    }
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
        if (encChangeCallbacks_[i].active && encChangeCallbacks_[i].callback) {
            encChangeCallbacks_[i].callback(connHandle, status);
        }
    }
}
 
void BluetoothController::onMtuExchange(uint16_t connHandle, uint16_t mtu) {
    int8_t slot = findConnectionSlot(connHandle);
    if (slot >= 0) {
        connections_[slot].mtu = mtu;
    }
}
 
void BluetoothController::onSubscribe(const struct ble_gap_event* event) {
    if (!event) return;
    LOG_I(TAG, "Subscribe: handle=%d attr=%d notify=%d indicate=%d",
          event->subscribe.conn_handle, event->subscribe.attr_handle,
          event->subscribe.cur_notify, event->subscribe.cur_indicate);
 
    // Find or allocate a subscribe entry
    int slot = -1;
    for (int i = 0; i < MAX_SUBSCRIBE_ENTRIES; i++) {
        if (subscribes_[i].active &&
            subscribes_[i].connHandle == event->subscribe.conn_handle &&
            subscribes_[i].attrHandle == event->subscribe.attr_handle) {
            slot = i;
            break;
        }
    }
    if (slot < 0) {
        for (int i = 0; i < MAX_SUBSCRIBE_ENTRIES; i++) {
            if (!subscribes_[i].active) { slot = i; break; }
        }
    }
    if (slot < 0) {
        LOG_W(TAG, "Subscribe table full");
        return;
    }
    subscribes_[slot].active = true;
    subscribes_[slot].connHandle = event->subscribe.conn_handle;
    subscribes_[slot].attrHandle = event->subscribe.attr_handle;
    subscribes_[slot].notify = event->subscribe.cur_notify != 0;
    subscribes_[slot].indicate = event->subscribe.cur_indicate != 0;
    if (!subscribes_[slot].notify && !subscribes_[slot].indicate) {
        subscribes_[slot].active = false;
    }
}

int8_t BluetoothController::getRssi() const {
    uint16_t handle = primaryConnHandle();
    if (handle == BLE_HS_CONN_HANDLE_NONE) {
        return 0;
    }
 
    int8_t rssi = 0;
    int rc = ble_gap_conn_rssi(handle, &rssi);
    return (rc == 0) ? rssi : 0;
}

void BluetoothController::onConnect(uint16_t connHandle, bool isPeripheral) {
    advertising_ = false;
    LOG_I(TAG, "Device connected (handle=%d, role=%s)",
          connHandle, isPeripheral ? "peripheral" : "central");
 
    // Track in connection table
    int slot = -1;
    for (int i = 0; i < MAX_CONNECTIONS; i++) {
        if (!connections_[i].active) { slot = i; break; }
    }
    if (slot >= 0) {
        connections_[slot].active = true;
        connections_[slot].handle = connHandle;
        connections_[slot].isPeripheral = isPeripheral;
        connections_[slot].mtu = 23;
    } else {
        LOG_W(TAG, "Connection table full, dropping handle %d", connHandle);
    }
 
    // As central, negotiate a larger ATT MTU up front. Service/characteristic
    // discovery that follows gives it time to complete before the first write,
    // so offers (which carry the sender name) and data chunks are not capped at
    // the 23-byte default.
    if (!isPeripheral) {
        ble_gattc_exchange_mtu(connHandle, nullptr, nullptr);
    }
 
    // Dispatch to registered listeners
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
        if (connCallbacks_[i].active && connCallbacks_[i].callback) {
            connCallbacks_[i].callback(connHandle);
        }
    }
}

void BluetoothController::onDisconnect(uint16_t connHandle, int reason) {
    LOG_I(TAG, "Device disconnected (handle=%d reason=%d)", connHandle, reason);
 
    bool wasPeripheral = true;
    int8_t slot = findConnectionSlot(connHandle);
    if (slot >= 0) {
        wasPeripheral = connections_[slot].isPeripheral;
        connections_[slot].active = false;
    }
 
    // Drop subscriptions associated with this connection
    for (uint8_t i = 0; i < MAX_SUBSCRIBE_ENTRIES; i++) {
        if (subscribes_[i].active && subscribes_[i].connHandle == connHandle) {
            subscribes_[i].active = false;
        }
    }
 
    // Dispatch to registered listeners
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
        if (disconnCallbacks_[i].active && disconnCallbacks_[i].callback) {
            disconnCallbacks_[i].callback(connHandle, reason);
        }
    }
 
    // Role-aware advertising restart: only if we were the peripheral.
    advertising_ = false;
    if (wasPeripheral) {
        startAdvertising();
    }
}

void BluetoothController::onSync() {
    synced_ = true;
 
    // Determine best address type
    int rc = ble_hs_util_ensure_addr(0);
    if (rc != 0) {
        LOG_E(TAG, "Failed to ensure address: %d", rc);
        return;
    }
 
    rc = ble_hs_id_infer_auto(0, &ownAddrType_);
    if (rc != 0) {
        LOG_E(TAG, "Failed to infer address type: %d", rc);
        ownAddrType_ = BLE_OWN_ADDR_PUBLIC;
    }
 
    uint8_t addr[6];
    ble_hs_id_copy_addr(ownAddrType_, addr, nullptr);
    LOG_I(TAG, "BLE synced, addr=%02X:%02X:%02X:%02X:%02X:%02X",
             addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
 
    // Auto-start advertising after sync
    startAdvertising();
}


void BluetoothController::startAdvertising() {
    if (!enabled_ || !synced_) return;
 
    // Stop any active advertising before reconfiguring. Query the real radio
    // state, not just the cached flag: a flag desync must never leave NimBLE
    // advertising while we restart it (which then returns BLE_HS_EALREADY and,
    // with isAdvertising() reading the flag, spins reconcile() into a re-advertise
    // loop).
    if (ble_gap_adv_active()) {
        ble_gap_adv_stop();
    }
    advertising_ = false;
 
    struct ble_gap_adv_params advParams = {};
    advParams.conn_mode = BLE_GAP_CONN_MODE_UND;
    advParams.disc_mode = BLE_GAP_DISC_MODE_GEN;
    advParams.itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN;
    advParams.itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MAX;
 
    // Split advertised UUIDs by width. Both 16-bit (e.g. HID 0x1812) and 128-bit
    // service UUIDs go into the primary PDU when they fit, so HOGP hosts recognize
    // the device and UUID-filtering scanners (which read only the primary payload)
    // match it. Overflowing fields spill into the scan response below.
    ble_uuid16_t uuid16s[MAX_ADV_UUIDS];
    ble_uuid128_t uuid128s[MAX_ADV_UUIDS];
    uint8_t num16 = 0, num128 = 0;
    for (uint8_t i = 0; i < advUuidCount_; i++) {
        if (advUuids_[i].type == BleUuid::UUID_16 && num16 < MAX_ADV_UUIDS) {
            uuid16s[num16].u.type = BLE_UUID_TYPE_16;
            uuid16s[num16].value = advUuids_[i].u16;
            num16++;
        } else if (advUuids_[i].type == BleUuid::UUID_128 && num128 < MAX_ADV_UUIDS) {
            uuid128s[num128].u.type = BLE_UUID_TYPE_128;
            memcpy(uuid128s[num128].value, advUuids_[i].u128, 16);
            num128++;
        }
    }
 
    // Primary advertising data: flags + appearance + 16-bit + 128-bit UUIDs + name.
    struct ble_hs_adv_fields fields = {};
    fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
    fields.tx_pwr_lvl_is_present = 1;
    fields.tx_pwr_lvl = BLE_HS_ADV_TX_PWR_LVL_AUTO;
    if (appearance_ != 0) {
        fields.appearance = appearance_;
        fields.appearance_is_present = 1;
    }
    if (num16 > 0) {
        fields.uuids16 = uuid16s;
        fields.num_uuids16 = num16;
        fields.uuids16_is_complete = 1;
    }
    if (num128 > 0) {
        fields.uuids128 = uuid128s;
        fields.num_uuids128 = num128;
        fields.uuids128_is_complete = 1;
    }
    fields.name = (uint8_t*)deviceName_;
    fields.name_len = strlen(deviceName_);
    fields.name_is_complete = 1;
 
    // The 31-byte primary PDU can overflow. Spill into the scan response in order
    // of decreasing importance: first the name (an active scanner fetches it from
    // the scan response anyway), then the 128-bit service UUIDs.
    bool nameInScanRsp = false;
    bool uuid128InScanRsp = false;
    int rc = ble_gap_adv_set_fields(&fields);
    if (rc == BLE_HS_EMSGSIZE) {
        fields.name = nullptr;
        fields.name_len = 0;
        fields.name_is_complete = 0;
        nameInScanRsp = true;
        rc = ble_gap_adv_set_fields(&fields);
    }
    if (rc == BLE_HS_EMSGSIZE && num128 > 0) {
        fields.uuids128 = nullptr;
        fields.num_uuids128 = 0;
        fields.uuids128_is_complete = 0;
        uuid128InScanRsp = true;
        rc = ble_gap_adv_set_fields(&fields);
    }
    if (rc != 0) {
        LOG_E(TAG, "Failed to set adv fields: %d", rc);
        return;
    }
 
    // Scan response carries whatever did not fit into the primary PDU plus
    // manufacturer data.
    if (nameInScanRsp || uuid128InScanRsp || mfgDataSet_) {
        struct ble_hs_adv_fields rsp = {};
 
        if (nameInScanRsp) {
            rsp.name = (uint8_t*)deviceName_;
            rsp.name_len = strlen(deviceName_);
            rsp.name_is_complete = 1;
        }
        if (uuid128InScanRsp) {
            rsp.uuids128 = uuid128s;
            rsp.num_uuids128 = num128;
            rsp.uuids128_is_complete = 1;
        }
 
        // Manufacturer-specific data (company ID + payload)
        uint8_t mfgAdvBuf[33];
        if (mfgDataSet_) {
            mfgAdvBuf[0] = mfgCompanyId_ & 0xFF;
            mfgAdvBuf[1] = (mfgCompanyId_ >> 8) & 0xFF;
            memcpy(mfgAdvBuf + 2, mfgData_, mfgDataLen_);
            rsp.mfg_data = mfgAdvBuf;
            rsp.mfg_data_len = mfgDataLen_ + 2;
        }
 
        rc = ble_gap_adv_rsp_set_fields(&rsp);
        if (rc != 0) {
            LOG_W(TAG, "Failed to set scan response: %d (continuing without)", rc);
        }
    }
 
    rc = ble_gap_adv_start(ownAddrType_, nullptr, BLE_HS_FOREVER,
                           &advParams, bleGapEventCallback, nullptr);
    if (rc != 0 && rc != BLE_HS_EALREADY) {
        LOG_E(TAG, "Failed to start advertising: %d", rc);
        return;
    }
 
    advertising_ = true;
    LOG_I(TAG, "Advertising started (%d service UUIDs)", advUuidCount_);
}

void BluetoothController::stopAdvertising() {
    if (!advertising_) return;
 
    ble_gap_adv_stop();
    advertising_ = false;
    LOG_I(TAG, "Advertising stopped");
}

void BluetoothController::onAdvComplete() {
    advertising_ = false;
}

void BluetoothController::setAppearance(uint16_t appearance) {
    if (appearance_ == appearance) return;
    appearance_ = appearance;
    if (enabled_ && synced_ && advertising_) {
        startAdvertising();
    }
}


bool BluetoothController::startScan(uint32_t durationMs, bool keepAdvertising) {
    if (!enabled_ || !synced_ || scanning_) return false;
 
    // ESP32-S3 NimBLE supports Peripheral + Observer multi-role, so advertising
    // and scanning can run concurrently. keepAdvertising leaves the beacon up so
    // two scanning badges still see each other; otherwise stop advertising first.
    bool wasAdvertising = advertising_;
    if (advertising_ && !keepAdvertising) {
        ble_gap_adv_stop();
        advertising_ = false;
        LOG_D(TAG, "Stopped advertising for scan");
    }
 
    // Clear previous results
    scanResultCount_ = 0;
    memset(scanResults_, 0, sizeof(scanResults_));
    memset(scanNameComplete_, 0, sizeof(scanNameComplete_));
 
    struct ble_gap_disc_params discParams = {};
    discParams.filter_duplicates = 0;  // Allow duplicates to get scan responses with names
    discParams.passive = 0;  // Active scan to get names
    discParams.itvl = 0;     // Use defaults
    discParams.window = 0;
    discParams.filter_policy = 0;
    discParams.limited = 0;
 
    // durationMs 0 means scan continuously until stopScan().
    int32_t duration = (durationMs == 0) ? BLE_HS_FOREVER
                                         : static_cast<int32_t>(durationMs);
    int rc = ble_gap_disc(ownAddrType_, duration, &discParams,
                          bleGapEventCallback, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "Failed to start scan: %d", rc);
        // Restore advertising if we stopped it
        if (wasAdvertising && !keepAdvertising) {
            startAdvertising();
        }
        return false;
    }
 
    // Only restore advertising on scan-complete if we actually stopped it.
    scanWasAdvertising_ = keepAdvertising ? false : wasAdvertising;
    scanning_ = true;
    LOG_I(TAG, "Scan started (%lu ms%s)", (unsigned long)durationMs,
          keepAdvertising ? ", adv kept" : "");
    return true;
}

void BluetoothController::stopScan() {
    if (!scanning_) return;
 
    ble_gap_disc_cancel();
    scanning_ = false;
    LOG_I(TAG, "Scan stopped");
}

static bool parseAdvName(const uint8_t* data, uint8_t dataLen,
                         char* name, size_t nameMaxLen, bool* isComplete = nullptr) {
    uint8_t pos = 0;
    while (pos < dataLen) {
        uint8_t len = data[pos];
        if (len == 0 || pos + len > dataLen) break;
        uint8_t type = data[pos + 1];
        // 0x09 = Complete Local Name, 0x08 = Shortened Local Name
        if (type == 0x09 || type == 0x08) {
            uint8_t nameLen = len - 1;
            size_t copyLen = (nameLen < nameMaxLen - 1) ? nameLen : nameMaxLen - 1;
            memcpy(name, &data[pos + 2], copyLen);
            name[copyLen] = '\0';
            if (isComplete) *isComplete = (type == 0x09);
            return true;
        }
        pos += len + 1;
    }
    return false;
}

static void fillScanResult(BleScanResult& result, const ble_gap_disc_desc* disc,
                           bool* outNameComplete) {
    memcpy(result.mac, disc->addr.val, 6);
    result.addrType = disc->addr.type;
    result.rssi = disc->rssi;
    result.name[0] = '\0';
 
    result.advDataLen = (disc->length_data <= sizeof(result.advData))
                         ? disc->length_data : sizeof(result.advData);
    memcpy(result.advData, disc->data, result.advDataLen);
 
    bool isComplete = false;
    parseAdvName(disc->data, disc->length_data, result.name, sizeof(result.name), &isComplete);
    const bool haveName = (result.name[0] != '\0');
    if (outNameComplete) *outNameComplete = haveName && isComplete;
    if (!haveName) {
        snprintf(result.name, sizeof(result.name), "%02X:%02X:%02X:%02X:%02X:%02X",
                 result.mac[5], result.mac[4], result.mac[3],
                 result.mac[2], result.mac[1], result.mac[0]);
    }
}
 
void BluetoothController::onScanResult(const ble_gap_disc_desc* disc) {
    if (!disc) return;
 
    // Update an existing entry (same MAC). Scan responses often carry the name.
    for (uint8_t i = 0; i < scanResultCount_; i++) {
        if (memcmp(scanResults_[i].mac, disc->addr.val, 6) == 0) {
            if (disc->rssi > scanResults_[i].rssi) {
                scanResults_[i].rssi = disc->rssi;
            }
            // Replace the name only with a non-empty one, and never downgrade a
            // Complete Local Name (0x09) to a Shortened one (0x08): devices that
            // advertise a short name in the primary PDU and the full name in the
            // scan response would otherwise flip-flop every interval.
            char parsedName[32];
            bool isComplete = false;
            if (parseAdvName(disc->data, disc->length_data, parsedName, sizeof(parsedName), &isComplete) &&
                parsedName[0] != '\0' &&
                (isComplete || !scanNameComplete_[i]) &&
                strcmp(parsedName, scanResults_[i].name) != 0) {
                strncpy(scanResults_[i].name, parsedName, sizeof(scanResults_[i].name) - 1);
                scanResults_[i].name[sizeof(scanResults_[i].name) - 1] = '\0';
                scanNameComplete_[i] = isComplete;
                LOG_D(TAG, "Name updated: %s (evt=0x%02X)", parsedName, disc->event_type);
            }
            return;
        }
    }
 
    // New device. Append while there is room; once full, keep the strongest by
    // replacing the weakest entry only when the newcomer has a higher RSSI.
    uint8_t slot;
    if (scanResultCount_ < MAX_SCAN_RESULTS) {
        slot = scanResultCount_++;
    } else {
        uint8_t weakest = 0;
        for (uint8_t i = 1; i < scanResultCount_; i++) {
            if (scanResults_[i].rssi < scanResults_[weakest].rssi) weakest = i;
        }
        if (disc->rssi <= scanResults_[weakest].rssi) return;
        slot = weakest;
    }
 
    bool nameComplete = false;
    fillScanResult(scanResults_[slot], disc, &nameComplete);
    scanNameComplete_[slot] = nameComplete;
    LOG_D(TAG, "Found: %s (RSSI %d) evt=0x%02X dlen=%d",
          scanResults_[slot].name, scanResults_[slot].rssi,
          disc->event_type, disc->length_data);
}

void BluetoothController::onScanComplete() {
    scanning_ = false;
    LOG_I(TAG, "Scan complete, found %d devices", scanResultCount_);
 
    // Restore advertising if it was active before scan
    if (scanWasAdvertising_) {
        scanWasAdvertising_ = false;
        startAdvertising();
    }
}

uint8_t BluetoothController::getScanResults(BleScanResult* results, uint8_t maxResults) {
    if (!results || maxResults == 0) return 0;
 
    uint8_t count = (scanResultCount_ < maxResults) ? scanResultCount_ : maxResults;
    memcpy(results, scanResults_, count * sizeof(BleScanResult));
    return count;
}


bool BluetoothController::registerGattService(const GattServiceDef& service,
                                              bool pluginReserved) {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    // Reuse the slot already holding this service UUID (idempotent re-register),
    // otherwise take a free slot. Registration is allowed while BLE is disabled:
    // the slot is stored and committed to NimBLE later from enable().
    // System modules draw from slots [0, PLUGIN_SERVICE_SLOT); a plugin draws
    // only from the reserved PLUGIN_SERVICE_SLOT, so neither can starve the other.
    ble_uuid_any_t wantUuid;
    convertUuid(service.uuid, wantUuid);
    const int firstSlot = pluginReserved ? PLUGIN_SERVICE_SLOT : 0;
    const int lastSlot  = pluginReserved ? MAX_REGISTERED_SERVICES : PLUGIN_SERVICE_SLOT;
    int slot = -1;
    for (int i = firstSlot; i < lastSlot; i++) {
        if (s_services[i].active && ble_uuid_cmp(&s_services[i].svcUuid.u, &wantUuid.u) == 0) {
            slot = i; break;
        }
    }
    if (slot < 0) {
        for (int i = firstSlot; i < lastSlot; i++) {
            if (!s_services[i].active) { slot = i; break; }
        }
    }
    if (slot < 0) {
        LOG_E(TAG, "No free GATT service slots (max %d)", MAX_REGISTERED_SERVICES);
        return false;
    }
 
    auto& s = s_services[slot];
    memset(&s, 0, sizeof(InternalService));
 
    // Convert service UUID
    convertUuid(service.uuid, s.svcUuid);
 
    // Convert characteristics
    uint8_t numChars = std::min(service.numCharacteristics, (uint8_t)MAX_CHARS_PER_SERVICE);
    s.numChars = numChars;
 
    for (uint8_t i = 0; i < numChars; i++) {
        const auto& src = service.characteristics[i];
        auto& dst = s.nimbleChars[i];
 
        convertUuid(src.uuid, s.charUuids[i]);
 
        dst.uuid = &s.charUuids[i].u;
        dst.access_cb = gattServiceAccessCb;
        dst.arg = &s;
        dst.descriptors = nullptr;
        dst.flags = mapProperties(src.properties, src.permissions);
        dst.min_key_size = 0;
        dst.val_handle = src.valueHandle;
 
        // Build per-characteristic descriptor list (e.g. HID Report Reference).
        uint8_t numDsc = src.numDescriptors;
        if (numDsc > MAX_DESCRIPTORS_PER_CHAR) numDsc = MAX_DESCRIPTORS_PER_CHAR;
        for (uint8_t d = 0; d < numDsc; d++) {
            const GattDescriptor& gd = src.descriptors[d];
            uint16_t uuid16 = 0;
            switch (gd.kind) {
                case GattDescriptorKind::REPORT_REFERENCE: uuid16 = 0x2908; break;
                default: uuid16 = 0; break;
            }
            if (uuid16 == 0) continue;
 
            s.dscUuids[i][d].u.type = BLE_UUID_TYPE_16;
            s.dscUuids[i][d].u16.u.type = BLE_UUID_TYPE_16;
            s.dscUuids[i][d].u16.value = uuid16;
 
            // Pack as [len][bytes]
            uint8_t copyLen = (gd.dataLen <= 4) ? gd.dataLen : 4;
            s.dscPacked[i][d][0] = copyLen;
            memcpy(&s.dscPacked[i][d][1], gd.data, copyLen);
 
            s.dscDefs[i][d].uuid = &s.dscUuids[i][d].u;
            s.dscDefs[i][d].att_flags = BLE_ATT_F_READ;
            s.dscDefs[i][d].min_key_size = 0;
            s.dscDefs[i][d].access_cb = gattStaticDescriptorAccessCb;
            s.dscDefs[i][d].arg = s.dscPacked[i][d];
        }
        if (numDsc > 0) {
            // Terminator descriptor
            memset(&s.dscDefs[i][numDsc], 0, sizeof(ble_gatt_dsc_def));
            dst.descriptors = s.dscDefs[i];
        }
 
        s.writeCallbacks[i] = src.onWrite;
        s.readCallbacks[i] = src.onRead;
    }
 
    // Terminator
    memset(&s.nimbleChars[numChars], 0, sizeof(ble_gatt_chr_def));
 
    // Build NimBLE service definition
    s.nimbleSvcs[0].type = BLE_GATT_SVC_TYPE_PRIMARY;
    s.nimbleSvcs[0].uuid = &s.svcUuid.u;
    s.nimbleSvcs[0].includes = nullptr;
    s.nimbleSvcs[0].characteristics = s.nimbleChars;
    memset(&s.nimbleSvcs[1], 0, sizeof(ble_gatt_svc_def));
 
    // Commit the slot; enable() (re)adds every active service through the one
    // correct init path.
    s.active = true;
 
    // BLE not enabled yet: enable() commits this slot later.
    if (!enabled_) {
        LOG_I(TAG, "GATT service stored, deferred until BLE enable (slot %d)", slot);
        return true;
    }
 
    // BLE already running: NimBLE cannot add a service to a started GATT server
    // in place (ble_svc_gap_init is not re-entrant and asserts on a second
    // call), so rebuild the whole stack through the proven disable()/enable()
    // path. This drops any active BLE connection.
    disable();
    if (!enable()) {
        s.active = false;
        LOG_E(TAG, "GATT service register: BLE restart failed (slot %d)", slot);
        return false;
    }
    LOG_I(TAG, "GATT service registered via BLE restart (slot %d, %d chars)", slot, numChars);
    return true;
}
 
bool BluetoothController::unregisterGattService(const BleUuid& serviceUuid) {
    cdc::core::RecursiveMutexGuard guard(lifecycleMutex_);
    ble_uuid_any_t wantUuid;
    convertUuid(serviceUuid, wantUuid);
    int slot = -1;
    for (int i = 0; i < MAX_REGISTERED_SERVICES; i++) {
        if (s_services[i].active && ble_uuid_cmp(&s_services[i].svcUuid.u, &wantUuid.u) == 0) {
            slot = i; break;
        }
    }
    if (slot < 0) return false;
 
    memset(&s_services[slot], 0, sizeof(InternalService));
 
    // While disabled there is no live GATT DB to rebuild; the slot is simply
    // dropped before the next enable() commits the remaining services.
    if (!enabled_) {
        LOG_I(TAG, "GATT service unregistered (slot %d, deferred)", slot);
        return true;
    }
 
    // Rebuild the live GATT DB through the proven disable()/enable() path (an
    // in-place ble_gatts_reset + ble_svc_gap_init re-init asserts in NimBLE).
    // This drops any active BLE connection.
    disable();
    if (!enable()) {
        LOG_E(TAG, "GATT service unregister: BLE restart failed (slot %d)", slot);
        return false;
    }
    LOG_I(TAG, "GATT service unregistered via BLE restart (slot %d)", slot);
    return true;
}

bool BluetoothController::sendNotification(uint16_t connHandle, uint16_t attrHandle,
                                            const uint8_t* data, uint16_t len) {
    if (!enabled_ || !data || len == 0) {
        return false;
    }
 
    auto notifyOne = [&](uint16_t handle) -> bool {
        // Check the per-connection CCCD table - skip peers that are not subscribed.
        bool subscribed = false;
        for (uint8_t i = 0; i < MAX_SUBSCRIBE_ENTRIES; i++) {
            if (subscribes_[i].active &&
                subscribes_[i].connHandle == handle &&
                subscribes_[i].attrHandle == attrHandle &&
                subscribes_[i].notify) {
                subscribed = true;
                break;
            }
        }
        if (!subscribed) return false;
 
        struct os_mbuf* om = ble_hs_mbuf_from_flat(data, len);
        if (!om) {
            LOG_E(TAG, "Failed to allocate mbuf for notification");
            return false;
        }
        int rc = ble_gatts_notify_custom(handle, attrHandle, om);
        if (rc != 0) {
            // NimBLE may or may not free the mbuf chain on failure; free
            // unconditionally to avoid a leak when rc != 0.
            os_mbuf_free_chain(om);
            return false;
        }
        return true;
    };
 
    if (connHandle == 0xFFFF || connHandle == BLE_HS_CONN_HANDLE_NONE) {
        bool any = false;
        for (uint8_t i = 0; i < MAX_CONNECTIONS; i++) {
            if (connections_[i].active) {
                if (notifyOne(connections_[i].handle)) any = true;
            }
        }
        return any;
    }
    return notifyOne(connHandle);
}

uint16_t BluetoothController::getMtu() const {
    uint16_t handle = primaryConnHandle();
    if (handle == BLE_HS_CONN_HANDLE_NONE) {
        return 20; // Default minimum BLE payload (ATT MTU 23 - 3 header bytes)
    }
    uint16_t mtu = ble_att_mtu(handle);
    return (mtu > 3) ? (mtu - 3) : 20;
}


bool BluetoothController::addAdvertisingUuid(const BleUuid& uuid) {
    // Check if already registered
    for (uint8_t i = 0; i < advUuidCount_; i++) {
        if (advUuids_[i] == uuid) return true;
    }
 
    if (advUuidCount_ >= MAX_ADV_UUIDS) {
        LOG_E(TAG, "Max advertising UUIDs reached (%d)", MAX_ADV_UUIDS);
        return false;
    }
 
    advUuids_[advUuidCount_++] = uuid;
    LOG_I(TAG, "Advertising UUID registered (%d total)", advUuidCount_);
 
    // Start or restart advertising to include new UUID
    if (enabled_ && synced_) {
        if (advertising_) {
            stopAdvertising();
        }
        startAdvertising();
    }
 
    return true;
}

void BluetoothController::removeAdvertisingUuid(const BleUuid& uuid) {
    for (uint8_t i = 0; i < advUuidCount_; i++) {
        if (advUuids_[i] == uuid) {
            // Shift remaining UUIDs
            for (uint8_t j = i; j < advUuidCount_ - 1; j++) {
                advUuids_[j] = advUuids_[j + 1];
            }
            advUuidCount_--;
            LOG_I(TAG, "Advertising UUID removed (%d remaining)", advUuidCount_);
 
            // Restart advertising without removed UUID
            if (advertising_) {
                stopAdvertising();
                startAdvertising();
            }
            return;
        }
    }
}


template <typename CB, size_t N>
static IBluetoothController::ListenerToken addListener(
    BluetoothController::ListenerSlot<CB> (&slots)[N], CB cb) {
    if (!cb) return IBluetoothController::INVALID_LISTENER;
    for (size_t i = 0; i < N; i++) {
        if (!slots[i].active) {
            slots[i].active = true;
            slots[i].callback = cb;
            return static_cast<IBluetoothController::ListenerToken>(i);
        }
    }
    return IBluetoothController::INVALID_LISTENER;
}

template <typename CB, size_t N>
static void removeListener(
    BluetoothController::ListenerSlot<CB> (&slots)[N],
    IBluetoothController::ListenerToken token) {
    if (token >= N) return;
    slots[token].active = false;
    slots[token].callback = nullptr;
}
 
IBluetoothController::ListenerToken
BluetoothController::addConnectionCallback(ConnectionCallback cb) {
    return addListener(connCallbacks_, cb);
}
 
IBluetoothController::ListenerToken
BluetoothController::addDisconnectionCallback(DisconnectionCallback cb) {
    return addListener(disconnCallbacks_, cb);
}
 
void BluetoothController::removeConnectionCallback(ListenerToken token) {
    removeListener(connCallbacks_, token);
}
 
void BluetoothController::removeDisconnectionCallback(ListenerToken token) {
    removeListener(disconnCallbacks_, token);
}


void BluetoothController::setPasskeyCallback(PasskeyCallback cb) {
    passkeyCb_ = cb;
}

void BluetoothController::setAuthCompleteCallback(AuthCompleteCallback cb) {
    authCompleteCb_ = cb;
}
 
IBluetoothController::ListenerToken
BluetoothController::addNumericComparisonCallback(NumericComparisonCallback cb) {
    return addListener(numCmpCallbacks_, cb);
}
 
void BluetoothController::removeNumericComparisonCallback(ListenerToken token) {
    removeListener(numCmpCallbacks_, token);
}
 
void BluetoothController::setNumericComparisonCallback(NumericComparisonCallback cb) {
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) numCmpCallbacks_[i].active = false;
    if (cb) addListener(numCmpCallbacks_, cb);
}
 
IBluetoothController::ListenerToken
BluetoothController::addEncryptionChangeCallback(EncChangeCallback cb) {
    return addListener(encChangeCallbacks_, cb);
}
 
void BluetoothController::removeEncryptionChangeCallback(ListenerToken token) {
    removeListener(encChangeCallbacks_, token);
}

bool BluetoothController::initiateSecurity(uint16_t connHandle) {
    int rc = ble_gap_security_initiate(connHandle);
    // BLE_HS_EALREADY: encryption already established or in progress.
    if (rc != 0 && rc != BLE_HS_EALREADY) {
        LOG_W(TAG, "ble_gap_security_initiate failed: %d", rc);
        return false;
    }
    return true;
}

bool BluetoothController::getPeerIdAddr(uint16_t connHandle, uint8_t addr[6],
                                        uint8_t* addrType) const {
    if (!addr || !addrType) return false;
    struct ble_gap_conn_desc desc = {};
    if (ble_gap_conn_find(connHandle, &desc) != 0) return false;
    std::memcpy(addr, desc.peer_id_addr.val, 6);
    *addrType = desc.peer_id_addr.type;
    return true;
}

void BluetoothController::forgetBond(const uint8_t addr[6], uint8_t addrType) {
    if (!addr) return;
    ble_addr_t peer = {};
    peer.type = addrType;
    std::memcpy(peer.val, addr, 6);
    // ble_gap_unpair() returns BLE_HS_EBUSY without deleting anything when the
    // peer distributed an IRK and advertising/discovery is active (the state in
    // the Bluetooth menu). Delete the bond records directly, as clearAllBonds()
    // does for all peers; the resolving-list entry is rebuilt empty on boot.
    int rc = ble_store_util_delete_peer(&peer);
    if (rc != 0) {
        LOG_W(TAG, "ble_store_util_delete_peer failed: %d", rc);
    } else {
        LOG_I(TAG, "Bond forgotten");
    }
}

uint8_t BluetoothController::getBondedDevices(BleBondInfo* out, uint8_t maxCount) const {
    if (!out || maxCount == 0) return 0;
 
    ble_addr_t peers[MAX_BONDS] = {};
    int numPeers = 0;
    if (ble_store_util_bonded_peers(peers, &numPeers, MAX_BONDS) != 0) {
        return 0;
    }
 
    uint8_t count = 0;
    for (int i = 0; i < numPeers && count < maxCount; i++) {
        std::memcpy(out[count].addr, peers[i].val, 6);
        out[count].addrType = peers[i].type;
        out[count].connected = false;
        for (uint8_t c = 0; c < MAX_CONNECTIONS; c++) {
            if (!connections_[c].active) continue;
            struct ble_gap_conn_desc desc = {};
            if (ble_gap_conn_find(connections_[c].handle, &desc) != 0) continue;
            if (desc.peer_id_addr.type == peers[i].type &&
                std::memcmp(desc.peer_id_addr.val, peers[i].val, 6) == 0) {
                out[count].connected = true;
                break;
            }
        }
        count++;
    }
    return count;
}

void BluetoothController::respondToNumericComparison(uint16_t connHandle, bool accept) {
    struct ble_sm_io pkey = {};
    pkey.action = BLE_SM_IOACT_NUMCMP;
    pkey.numcmp_accept = accept ? 1 : 0;
    int rc = ble_sm_inject_io(connHandle, &pkey);
    if (rc != 0) {
        LOG_E(TAG, "ble_sm_inject_io failed: %d", rc);
    }
    LOG_I(TAG, "Pairing %s", accept ? "accepted" : "rejected");
}

void BluetoothController::onPasskeyAction(uint16_t connHandle,
                                            const ble_gap_passkey_params* params) {
    if (!params) return;
 
    switch (params->action) {
        case BLE_SM_IOACT_NUMCMP: {
            LOG_I(TAG, "Numeric comparison: %06lu", (unsigned long)params->numcmp);
            LOG_I(TAG, "%s stack free: %lu words", pcTaskGetName(nullptr),
                  (unsigned long)uxTaskGetStackHighWaterMark(nullptr));
            bool dispatched = false;
            for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
                if (numCmpCallbacks_[i].active && numCmpCallbacks_[i].callback) {
                    numCmpCallbacks_[i].callback(connHandle, params->numcmp);
                    dispatched = true;
                }
            }
            if (!dispatched) {
                // No callback registered: reject by default for safety.
                LOG_W(TAG, "No numeric comparison callback registered, rejecting");
                respondToNumericComparison(connHandle, false);
            }
            break;
        }
 
        case BLE_SM_IOACT_DISP:
            LOG_I(TAG, "Display passkey: %06lu", (unsigned long)params->numcmp);
            if (passkeyCb_) {
                passkeyCb_(params->numcmp);
            }
            break;
 
        default:
            LOG_W(TAG, "Unhandled passkey action: %d", params->action);
            break;
    }
}


bool BluetoothController::setAdvertisingManufacturerData(uint16_t companyId,
                                                          const uint8_t* data, uint16_t len) {
    if (!data || len > sizeof(mfgData_)) return false;
 
    memcpy(mfgData_, data, len);
    mfgDataLen_ = len;
    mfgCompanyId_ = companyId;
    mfgDataSet_ = true;
 
    if (advertising_) {
        stopAdvertising();
        startAdvertising();
    }
    return true;
}

void BluetoothController::clearAdvertisingManufacturerData() {
    mfgDataSet_ = false;
    mfgDataLen_ = 0;
 
    if (advertising_) {
        stopAdvertising();
        startAdvertising();
    }
}


int gattcChrDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                    const struct ble_gatt_chr* chr, void* arg) {
    (void)arg;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    if (error->status == 0 && chr) {
        auto& svc = ctrl->discoveredSvc_;
        if (svc.numCharacteristics < IBluetoothController::DiscoveredService::MAX_DISCOVERED_CHARS) {
            auto& dc = svc.characteristics[svc.numCharacteristics];
            // Use NimBLE's typed helper to safely extract 16-bit UUIDs from
            // either BLE_UUID_TYPE_16 or BLE_UUID_TYPE_32 variants. Anything
            // larger falls back to 128-bit.
            if (chr->uuid.u.type == BLE_UUID_TYPE_16) {
                dc.uuid = BleUuid::from16(ble_uuid_u16(&chr->uuid.u));
            } else if (chr->uuid.u.type == BLE_UUID_TYPE_32) {
                // No native 32-bit support in our generic UUID; convert to 128-bit per BT spec.
                uint8_t u128[16] = { 0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00,
                                      0x00, 0x80, 0x00, 0x10, 0x00, 0x00,
                                      0, 0, 0, 0 };
                uint32_t v = chr->uuid.u32.value;
                u128[12] = v & 0xFF;
                u128[13] = (v >> 8) & 0xFF;
                u128[14] = (v >> 16) & 0xFF;
                u128[15] = (v >> 24) & 0xFF;
                dc.uuid = BleUuid::from128(u128);
            } else {
                dc.uuid = BleUuid::from128(chr->uuid.u128.value);
            }
            dc.valueHandle = chr->val_handle;
            dc.properties = chr->properties;
            svc.numCharacteristics++;
        }
    } else if (error->status == BLE_HS_EDONE) {
        LOG_I(TAG, "Char discovery done (%d chars)", ctrl->discoveredSvc_.numCharacteristics);
        for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
            if (ctrl->svcDiscoveryCallbacks_[i].active &&
                ctrl->svcDiscoveryCallbacks_[i].callback) {
                ctrl->svcDiscoveryCallbacks_[i].callback(
                    connHandle, &ctrl->discoveredSvc_, true);
            }
        }
    } else {
        LOG_E(TAG, "Char discovery error: %d", error->status);
        for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
            if (ctrl->svcDiscoveryCallbacks_[i].active &&
                ctrl->svcDiscoveryCallbacks_[i].callback) {
                ctrl->svcDiscoveryCallbacks_[i].callback(connHandle, nullptr, true);
            }
        }
    }
 
    return 0;
}

int gattcSvcDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                    const struct ble_gatt_svc* service, void* arg) {
    (void)arg;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    auto dispatchFailure = [&]() {
        for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
            if (ctrl->svcDiscoveryCallbacks_[i].active &&
                ctrl->svcDiscoveryCallbacks_[i].callback) {
                ctrl->svcDiscoveryCallbacks_[i].callback(connHandle, nullptr, true);
            }
        }
    };
 
    if (error->status == 0 && service) {
        ctrl->discoverSvcStart_ = service->start_handle;
        ctrl->discoverSvcEnd_ = service->end_handle;
        LOG_I(TAG, "Service found: handles %d-%d",
              service->start_handle, service->end_handle);
    } else if (error->status == BLE_HS_EDONE) {
        if (ctrl->discoverSvcStart_ != 0) {
            int rc = ble_gattc_disc_all_chrs(connHandle,
                                              ctrl->discoverSvcStart_,
                                              ctrl->discoverSvcEnd_,
                                              gattcChrDiscCb, nullptr);
            if (rc != 0) {
                LOG_E(TAG, "ble_gattc_disc_all_chrs failed: %d", rc);
                dispatchFailure();
            }
        } else {
            LOG_W(TAG, "Service not found on remote device");
            dispatchFailure();
        }
    } else {
        LOG_E(TAG, "Service discovery error: %d", error->status);
        dispatchFailure();
    }
 
    return 0;
}

int gattcReadCb(uint16_t connHandle, const struct ble_gatt_error* error,
                 struct ble_gatt_attr* attr, void* arg) {
    (void)arg;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    if (error->status == 0 && attr && attr->om) {
        uint16_t len = OS_MBUF_PKTLEN(attr->om);
        if (len > sizeof(s_gattAccessBuf)) len = sizeof(s_gattAccessBuf);
        ble_hs_mbuf_to_flat(attr->om, s_gattAccessBuf, len, nullptr);
        for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
            if (ctrl->charReadCallbacks_[i].active &&
                ctrl->charReadCallbacks_[i].callback) {
                ctrl->charReadCallbacks_[i].callback(
                    connHandle, attr->handle, s_gattAccessBuf, len);
            }
        }
    } else {
        LOG_E(TAG, "GATT read failed: %d", error->status);
        for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
            if (ctrl->charReadCallbacks_[i].active &&
                ctrl->charReadCallbacks_[i].callback) {
                ctrl->charReadCallbacks_[i].callback(connHandle, 0, nullptr, 0);
            }
        }
    }
 
    return 0;
}

int gattcWriteCb(uint16_t connHandle, const struct ble_gatt_error* error,
                   struct ble_gatt_attr* attr, void* arg) {
    (void)arg;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    uint16_t handle = attr ? attr->handle : 0;
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) {
        if (ctrl->writeCompleteCallbacks_[i].active &&
            ctrl->writeCompleteCallbacks_[i].callback) {
            ctrl->writeCompleteCallbacks_[i].callback(connHandle, handle, error->status);
        }
    }
 
    return 0;
}


bool BluetoothController::connect(const uint8_t* addr, uint8_t addrType) {
    if (!enabled_ || !synced_ || !addr) return false;
 
    // Must stop advertising to connect as central
    if (advertising_) {
        ble_gap_adv_stop();
        advertising_ = false;
    }
 
    ble_addr_t bleAddr;
    bleAddr.type = addrType;
    memcpy(bleAddr.val, addr, 6);
 
    int rc = ble_gap_connect(ownAddrType_, &bleAddr, 10000, nullptr,
                             bleGapEventCallback, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "ble_gap_connect failed: %d", rc);
        startAdvertising();
        return false;
    }
 
    LOG_I(TAG, "Connecting to %02X:%02X:%02X:%02X:%02X:%02X",
          addr[5], addr[4], addr[3], addr[2], addr[1], addr[0]);
    return true;
}

void BluetoothController::cancelConnect() {
    ble_gap_conn_cancel();
}

bool BluetoothController::discoverServiceByUuid(uint16_t connHandle, const BleUuid& uuid) {
    if (!enabled_) return false;
 
    // Reset discovery state
    memset(&discoveredSvc_, 0, sizeof(discoveredSvc_));
    discoveredSvc_.uuid = uuid;
    discoverTargetUuid_ = uuid;
    discoverSvcStart_ = 0;
    discoverSvcEnd_ = 0;
 
    ble_uuid_any_t nimbleUuid;
    convertUuid(uuid, nimbleUuid);
 
    int rc = ble_gattc_disc_svc_by_uuid(connHandle, &nimbleUuid.u,
                                          gattcSvcDiscCb, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "ble_gattc_disc_svc_by_uuid failed: %d", rc);
        return false;
    }
 
    LOG_I(TAG, "Service discovery started (connHandle=%d)", connHandle);
    return true;
}

bool BluetoothController::writeCharacteristic(uint16_t connHandle, uint16_t attrHandle,
                                               const uint8_t* data, uint16_t len,
                                               bool withResponse) {
    if (!enabled_ || !data || len == 0) return false;
 
    int rc;
    if (withResponse) {
        rc = ble_gattc_write_flat(connHandle, attrHandle, data, len,
                                   gattcWriteCb, nullptr);
    } else {
        // Write-without-response has no confirmation event. Do not invoke
        // writeCompleteCallback synchronously - modules treating that as a
        // TX confirmation would misreport delivery.
        rc = ble_gattc_write_no_rsp_flat(connHandle, attrHandle, data, len);
    }
 
    if (rc != 0) {
        LOG_E(TAG, "GATT write failed: %d", rc);
        return false;
    }
 
    return true;
}

bool BluetoothController::readCharacteristic(uint16_t connHandle, uint16_t attrHandle) {
    if (!enabled_) return false;
 
    int rc = ble_gattc_read(connHandle, attrHandle, gattcReadCb, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "ble_gattc_read failed: %d", rc);
        return false;
    }
 
    return true;
}

bool BluetoothController::enableNotifications(uint16_t connHandle, uint16_t cccdHandle) {
    if (!enabled_) return false;
 
    uint8_t val[2] = { 0x01, 0x00 };
    int rc = ble_gattc_write_flat(connHandle, cccdHandle, val, sizeof(val),
                                   gattcWriteCb, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "Enable notifications failed: %d", rc);
        return false;
    }
 
    LOG_I(TAG, "Notifications enabled (cccd=%d)", cccdHandle);
    return true;
}

int gattcDscDiscCb(uint16_t connHandle, const struct ble_gatt_error* error,
                    uint16_t chr_val_handle, const struct ble_gatt_dsc* dsc, void* arg) {
    (void)arg;
    (void)chr_val_handle;
    auto* ctrl = BluetoothController::instance_;
    if (!ctrl) return 0;
 
    if (error->status == 0 && dsc) {
        if (dsc->uuid.u.type == BLE_UUID_TYPE_16 && dsc->uuid.u16.value == 0x2902 &&
            ctrl->pendingSubCccdHandle_ == 0) {
            ctrl->pendingSubCccdHandle_ = dsc->handle;
        }
        return 0;
    }
 
    // Discovery finished (BLE_HS_EDONE) or failed: write whatever we found.
    uint16_t cccd = ctrl->pendingSubCccdHandle_;
    if (cccd == 0) {
        cccd = static_cast<uint16_t>(ctrl->pendingSubValueHandle_ + 1);
        LOG_W(TAG, "No CCCD found for handle %d, falling back to %d",
              ctrl->pendingSubValueHandle_, cccd);
    }
    ctrl->pendingSubValueHandle_ = 0;
    ctrl->pendingSubCccdHandle_ = 0;
    ctrl->enableNotifications(connHandle, cccd);
    return 0;
}

bool BluetoothController::subscribeToCharacteristic(uint16_t connHandle, uint16_t valueHandle) {
    if (!enabled_ || valueHandle == 0) return false;
 
    // The descriptor range ends where the last-discovered service ends; without
    // a prior service discovery there is no bound, so use the +1 heuristic.
    if (discoverSvcEnd_ == 0 || valueHandle >= discoverSvcEnd_) {
        return enableNotifications(connHandle, static_cast<uint16_t>(valueHandle + 1));
    }
 
    pendingSubValueHandle_ = valueHandle;
    pendingSubCccdHandle_ = 0;
    int rc = ble_gattc_disc_all_dscs(connHandle, valueHandle, discoverSvcEnd_,
                                     gattcDscDiscCb, nullptr);
    if (rc != 0) {
        LOG_E(TAG, "ble_gattc_disc_all_dscs failed: %d", rc);
        pendingSubValueHandle_ = 0;
        return enableNotifications(connHandle, static_cast<uint16_t>(valueHandle + 1));
    }
    return true;
}

void BluetoothController::disconnectHandle(uint16_t connHandle) {
    ble_gap_terminate(connHandle, BLE_ERR_REM_USER_CONN_TERM);
}
 
IBluetoothController::ListenerToken
BluetoothController::addServiceDiscoveryCallback(ServiceDiscoveryCallback cb) {
    return addListener(svcDiscoveryCallbacks_, cb);
}
IBluetoothController::ListenerToken
BluetoothController::addCharacteristicReadCallback(CharacteristicReadCallback cb) {
    return addListener(charReadCallbacks_, cb);
}
IBluetoothController::ListenerToken
BluetoothController::addNotificationCallback(NotificationCallback cb) {
    return addListener(notifyCallbacks_, cb);
}
IBluetoothController::ListenerToken
BluetoothController::addWriteCompleteCallback(WriteCompleteCallback cb) {
    return addListener(writeCompleteCallbacks_, cb);
}
 
void BluetoothController::removeServiceDiscoveryCallback(ListenerToken t) {
    removeListener(svcDiscoveryCallbacks_, t);
}
void BluetoothController::removeCharacteristicReadCallback(ListenerToken t) {
    removeListener(charReadCallbacks_, t);
}
void BluetoothController::removeNotificationCallback(ListenerToken t) {
    removeListener(notifyCallbacks_, t);
}
void BluetoothController::removeWriteCompleteCallback(ListenerToken t) {
    removeListener(writeCompleteCallbacks_, t);
}
 
void BluetoothController::setServiceDiscoveryCallback(ServiceDiscoveryCallback cb) {
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) svcDiscoveryCallbacks_[i].active = false;
    if (cb) addListener(svcDiscoveryCallbacks_, cb);
}
void BluetoothController::setCharacteristicReadCallback(CharacteristicReadCallback cb) {
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) charReadCallbacks_[i].active = false;
    if (cb) addListener(charReadCallbacks_, cb);
}
void BluetoothController::setNotificationCallback(NotificationCallback cb) {
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) notifyCallbacks_[i].active = false;
    if (cb) addListener(notifyCallbacks_, cb);
}
void BluetoothController::setWriteCompleteCallback(WriteCompleteCallback cb) {
    for (uint8_t i = 0; i < MAX_CONN_CALLBACKS; i++) writeCompleteCallbacks_[i].active = false;
    if (cb) addListener(writeCompleteCallbacks_, cb);
}

IBluetoothController* getBluetoothControllerInstance() {
    static BluetoothController* g_bluetoothController = new BluetoothController();
    return g_bluetoothController;
}
 
} // namespace cdc::hal
 
#else // NimBLE not enabled - stub implementation
 
#include "esp_mac.h"
#include <cstring>
 
namespace cdc::hal {

class BluetoothControllerStub : public IBluetoothController {
public:
    bool init() override {
        LOG_W(TAG, "Bluetooth disabled (NimBLE not configured)");
        state_ = core::ServiceState::INITIALIZED;
        return true;
    }

    bool start() override {
        state_ = core::ServiceState::STARTED;
        return true;
    }
    void stop() override { state_ = core::ServiceState::STOPPED; }
    core::ServiceState getState() const override { return state_; }
    const char* getName() const override { return "bluetooth"; }
 
    bool enable() override {
        LOG_W(TAG, "Cannot enable - NimBLE not configured in sdkconfig");
        return false;
    }
    void disable() override {}
    bool isEnabled() const override { return false; }
    bool getMacAddress(uint8_t* mac) const override {
        if (mac) esp_read_mac(mac, ESP_MAC_BT);
        return mac != nullptr;
    }

    void setDeviceName(const char* name) override {
        if (name) {
            strncpy(deviceName_, name, sizeof(deviceName_) - 1);
        }
    }
    const char* getDeviceName() const override { return deviceName_; }
    bool isConnected() const override { return false; }
    void disconnect() override {}
    int8_t getRssi() const override { return 0; }
 
private:
    core::ServiceState state_ = core::ServiceState::UNINITIALIZED;
    char deviceName_[32] = "CDC Badge";
};
 
static BluetoothControllerStub g_bluetoothController;

IBluetoothController* getBluetoothControllerInstance() {
    return &g_bluetoothController;
}
 
} // namespace cdc::hal
 
#endif // CONFIG_BT_ENABLED && CONFIG_BT_NIMBLE_ENABLED