#include "ble_service.h"
#include "ble_service_config.h"

#include <cstring>
#include <esp_log.h>
#include <esp_bt.h>
#include <esp_bt_main.h>
#include <esp_gap_ble_api.h>
#include <esp_gatts_api.h>
#include <esp_gatt_common_api.h>
#include <cJSON.h>

static const char* TAG = BLE_JSON_TAG;

BleJsonService* BleJsonService::instance_ = nullptr;

// ============================================================
// 命令队列消息结构
// ============================================================
struct BleJsonCmdMsg {
    char* json_str;   // 动态分配的 JSON 字符串，需要 free
    uint16_t len;
};

// ============================================================
// UUID 定义
// ============================================================
static esp_bt_uuid_t service_uuid = {
    .len = ESP_UUID_LEN_16,
    .uuid = {.uuid16 = BLE_JSON_SERVICE_UUID},
};

static esp_bt_uuid_t write_char_uuid = {
    .len = ESP_UUID_LEN_16,
    .uuid = {.uuid16 = BLE_JSON_CHAR_WRITE_UUID},
};

static esp_bt_uuid_t notify_char_uuid = {
    .len = ESP_UUID_LEN_16,
    .uuid = {.uuid16 = BLE_JSON_CHAR_NOTIFY_UUID},
};

static esp_bt_uuid_t status_char_uuid = {
    .len = ESP_UUID_LEN_16,
    .uuid = {.uuid16 = BLE_JSON_CHAR_STATUS_UUID},
};

// CCCD 描述符 UUID (标准 0x2902)
static esp_bt_uuid_t cccd_uuid = {
    .len = ESP_UUID_LEN_16,
    .uuid = {.uuid16 = ESP_GATT_UUID_CHAR_CLIENT_CONFIG},
};

// ============================================================
// Characteristic 属性值缓冲区 (Auto Response 使用)
// ============================================================
static uint8_t write_char_val[BLE_JSON_CHAR_VAL_MAX_LEN] = {0};
static uint8_t notify_char_val[BLE_JSON_CHAR_VAL_MAX_LEN] = {0};
static uint8_t status_char_val[BLE_JSON_CHAR_VAL_MAX_LEN] = {0};
static uint8_t cccd_val[2] = {0x00, 0x00};

static esp_attr_value_t write_char_attr = {
    .attr_max_len = BLE_JSON_CHAR_VAL_MAX_LEN,
    .attr_len = 1,
    .attr_value = write_char_val,
};

static esp_attr_value_t notify_char_attr = {
    .attr_max_len = BLE_JSON_CHAR_VAL_MAX_LEN,
    .attr_len = 1,
    .attr_value = notify_char_val,
};

static esp_attr_value_t status_char_attr = {
    .attr_max_len = BLE_JSON_CHAR_VAL_MAX_LEN,
    .attr_len = 1,
    .attr_value = status_char_val,
};

static esp_attr_value_t cccd_attr = {
    .attr_max_len = 2,
    .attr_len = 2,
    .attr_value = cccd_val,
};

// ============================================================
// 广播数据
// ============================================================
static esp_ble_adv_params_t ble_json_adv_params = {
    .adv_int_min = BLE_JSON_ADV_INT_MIN,
    .adv_int_max = BLE_JSON_ADV_INT_MAX,
    .adv_type = ADV_TYPE_IND,
    .own_addr_type = BLE_ADDR_TYPE_PUBLIC,
    .peer_addr = {0},
    .peer_addr_type = BLE_ADDR_TYPE_PUBLIC,
    .channel_map = ADV_CHNL_ALL,
    .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};

// ============================================================
// 构造 / 析构
// ============================================================

BleJsonService::BleJsonService() {
    instance_ = this;
}

BleJsonService::~BleJsonService() {
    Stop();
    if (instance_ == this) {
        instance_ = nullptr;
    }
}

// ============================================================
// Initialize — 注册 GATTS App
// ============================================================

bool BleJsonService::Initialize() {
    if (initialized_) {
        ESP_LOGW(TAG, "Already initialized");
        return true;
    }

    esp_err_t ret;

    // 检查 Bluedroid 栈是否已启动 (可能由 BLE 配网模块启动过)
    // 如果未启动，则自行初始化整个 BLE 栈
    esp_bluedroid_status_t bt_status = esp_bluedroid_get_status();
    if (bt_status != ESP_BLUEDROID_STATUS_ENABLED) {
        ESP_LOGI(TAG, "Bluedroid not enabled, initializing BLE stack...");

        // 释放经典蓝牙内存
        esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);

        // 初始化 BT Controller
        esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
        ret = esp_bt_controller_init(&bt_cfg);
        if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
            ESP_LOGE(TAG, "BT controller init failed: %s", esp_err_to_name(ret));
            return false;
        }

        ret = esp_bt_controller_enable(ESP_BT_MODE_BLE);
        if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
            ESP_LOGE(TAG, "BT controller enable failed: %s", esp_err_to_name(ret));
            return false;
        }

        // 初始化 Bluedroid
        ret = esp_bluedroid_init();
        if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
            ESP_LOGE(TAG, "Bluedroid init failed: %s", esp_err_to_name(ret));
            return false;
        }

        ret = esp_bluedroid_enable();
        if (ret != ESP_OK && ret != ESP_ERR_INVALID_STATE) {
            ESP_LOGE(TAG, "Bluedroid enable failed: %s", esp_err_to_name(ret));
            return false;
        }

        ESP_LOGI(TAG, "BLE stack initialized successfully");
    }

    // 设置 MTU
    ret = esp_ble_gatt_set_local_mtu(BLE_JSON_LOCAL_MTU);
    if (ret != ESP_OK) {
        ESP_LOGW(TAG, "Set MTU failed: %s (may already be set)", esp_err_to_name(ret));
    }

    // 注册 GAP 回调 (广播事件需要此回调才能启动)
    // 注意: ESP-IDF 仅支持一个全局 GAP 回调，此处会覆盖配网模块的回调
    // 但 BLE 配网流程在 StartNetwork() 中已完成，不影响后续使用
    ret = esp_ble_gap_register_callback(GapEventHandler);
    if (ret != ESP_OK) {
        ESP_LOGW(TAG, "GAP callback register: %s", esp_err_to_name(ret));
    }

    // 注册 GATTS 回调 (全局只能有一个，但 Bluedroid 支持按 gatts_if 分发)
    // 配网模块已经注册了自己的回调，我们通过 app_register 获得独立的 gatts_if
    ret = esp_ble_gatts_register_callback(GattsEventHandler);
    if (ret != ESP_OK) {
        ESP_LOGW(TAG, "GATTS callback register: %s (may share with provisioning)", esp_err_to_name(ret));
    }

    // 注册独立的 GATTS App，获得自己的 gatts_if
    ret = esp_ble_gatts_app_register(BLE_JSON_APP_ID);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "GATTS app register failed: %s", esp_err_to_name(ret));
        return false;
    }

    // 创建命令处理队列和任务
    cmd_queue_ = xQueueCreate(BLE_JSON_CMD_QUEUE_SIZE, sizeof(BleJsonCmdMsg));
    if (!cmd_queue_) {
        ESP_LOGE(TAG, "Failed to create cmd queue");
        return false;
    }

    xTaskCreate(CmdProcessTask, "ble_json_cmd", 6144, this, 5, &cmd_task_handle_);

    initialized_ = true;
    ESP_LOGI(TAG, "Initialized successfully (App ID=%d)", BLE_JSON_APP_ID);
    return true;
}

// ============================================================
// Start — 启动广播
// ============================================================

bool BleJsonService::Start(const char* device_name) {
    if (!initialized_) {
        ESP_LOGE(TAG, "Not initialized");
        return false;
    }

    device_name_ = device_name ? device_name : BLE_JSON_DEVICE_NAME;

    // 设置设备名称
    esp_ble_gap_set_device_name(device_name_.c_str());

    StartAdvertising();
    ESP_LOGI(TAG, "Started, device name: %s", device_name_.c_str());
    return true;
}

// ============================================================
// Stop
// ============================================================

void BleJsonService::Stop() {
    if (!initialized_) return;

    esp_ble_gap_stop_advertising();

    if (cmd_task_handle_) {
        vTaskDelete(cmd_task_handle_);
        cmd_task_handle_ = nullptr;
    }
    if (cmd_queue_) {
        // 清空队列中残留的消息
        BleJsonCmdMsg msg;
        while (xQueueReceive(cmd_queue_, &msg, 0) == pdTRUE) {
            free(msg.json_str);
        }
        vQueueDelete(cmd_queue_);
        cmd_queue_ = nullptr;
    }

    connected_ = false;
    notify_enabled_ = false;
    initialized_ = false;
    ESP_LOGI(TAG, "Stopped");
}

// ============================================================
// SendResponse — 构建 JSON 响应并通过 NOTIFY 发送
// ============================================================

bool BleJsonService::SendResponse(const std::string& cmd, int msg_id, int code,
                                  const char* msg, cJSON* data) {
    if (!connected_ || !notify_enabled_) {
        ESP_LOGW(TAG, "Cannot send: connected=%d notify=%d", connected_, notify_enabled_);
        return false;
    }

    cJSON* root = cJSON_CreateObject();
    cJSON_AddStringToObject(root, "cmd", cmd.c_str());
    cJSON_AddNumberToObject(root, "id", msg_id);
    cJSON_AddNumberToObject(root, "code", code);
    if (msg) {
        cJSON_AddStringToObject(root, "msg", msg);
    }
    if (data) {
        cJSON_AddItemReferenceToObject(root, "data", data);
    }

    char* json_str = cJSON_PrintUnformatted(root);
    cJSON_Delete(root);

    if (!json_str) {
        ESP_LOGE(TAG, "JSON print failed");
        return false;
    }

    uint16_t len = strlen(json_str);
    ESP_LOGI(TAG, "TX(%d): %s", len, json_str);

    bool ok = SendNotify(json_str, len);
    free(json_str);
    return ok;
}

// ============================================================
// SendEvent — 构建主动推送事件
// ============================================================

bool BleJsonService::SendEvent(const std::string& event_type, cJSON* data) {
    if (!connected_ || !notify_enabled_) {
        return false;
    }

    cJSON* root = cJSON_CreateObject();
    cJSON_AddStringToObject(root, "cmd", "event");

    cJSON* event_data = data ? cJSON_Duplicate(data, true) : cJSON_CreateObject();
    cJSON_AddStringToObject(event_data, "type", event_type.c_str());
    cJSON_AddItemToObject(root, "data", event_data);

    char* json_str = cJSON_PrintUnformatted(root);
    cJSON_Delete(root);

    if (!json_str) return false;

    uint16_t len = strlen(json_str);
    ESP_LOGI(TAG, "TX event(%d): %s", len, json_str);

    bool ok = SendNotify(json_str, len);
    free(json_str);
    return ok;
}

// ============================================================
// SendNotify — 底层 NOTIFY 发送
// ============================================================

bool BleJsonService::SendNotify(const char* json_str, uint16_t len) {
    if (gatts_if_ == ESP_GATT_IF_NONE || !connected_) {
        return false;
    }

    // 检查是否超过 MTU
    uint16_t max_payload = mtu_ - 3;
    if (len > max_payload) {
        ESP_LOGW(TAG, "Data len %d exceeds MTU payload %d, truncating", len, max_payload);
        len = max_payload;
    }

    esp_err_t ret = esp_ble_gatts_send_indicate(
        gatts_if_, conn_id_, notify_char_handle_,
        len, (uint8_t*)json_str, false  // false = notification, true = indication
    );

    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Send notify failed: %s", esp_err_to_name(ret));
        return false;
    }
    return true;
}

// ============================================================
// StartAdvertising
// ============================================================

void BleJsonService::StartAdvertising() {
    // 广播包：不放名称，避免超过31字节导致手机系统蓝牙搜索不到
    esp_ble_adv_data_t adv_data = {};
    adv_data.set_scan_rsp = false;
    adv_data.include_name = false;  // 名称放在 Scan Response 中
    adv_data.include_txpower = true;
    adv_data.min_interval = 0x0006;
    adv_data.max_interval = 0x0010;
    adv_data.appearance = 0x00;
    adv_data.manufacturer_len = 0;
    adv_data.p_manufacturer_data = nullptr;
    adv_data.service_data_len = 0;
    adv_data.p_service_data = nullptr;
    adv_data.service_uuid_len = sizeof(uint16_t);
    adv_data.p_service_uuid = (uint8_t*)&service_uuid.uuid.uuid16;
    adv_data.flag = (ESP_BLE_ADV_FLAG_GEN_DISC | ESP_BLE_ADV_FLAG_BREDR_NOT_SPT);

    esp_ble_gap_config_adv_data(&adv_data);

    // Scan Response：放完整设备名称，手机扫描时会主动请求
    esp_ble_adv_data_t scan_rsp = {};
    scan_rsp.set_scan_rsp = true;
    scan_rsp.include_name = true;
    scan_rsp.include_txpower = false;
    esp_ble_gap_config_adv_data(&scan_rsp);
}

// ============================================================
// GAP 事件回调
// ============================================================

void BleJsonService::GapEventHandler(esp_gap_ble_cb_event_t event,
                                     esp_ble_gap_cb_param_t* param) {
    switch (event) {
        case ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT:
        case ESP_GAP_BLE_SCAN_RSP_DATA_SET_COMPLETE_EVT: {
            // 两个事件都到达后才启动广播
            static uint8_t adv_config_done = 0;
            if (event == ESP_GAP_BLE_ADV_DATA_SET_COMPLETE_EVT) {
                adv_config_done |= 0x01;
            } else {
                adv_config_done |= 0x02;
            }
            if (adv_config_done == 0x03) {
                adv_config_done = 0;  // 重置，供下次重新广播使用
                esp_ble_gap_start_advertising(&ble_json_adv_params);
            }
            break;
        }
        case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
            if (param->adv_start_cmpl.status != ESP_BT_STATUS_SUCCESS) {
                ESP_LOGE(TAG, "Advertising start failed: %d", param->adv_start_cmpl.status);
            } else {
                ESP_LOGI(TAG, "Advertising started");
            }
            break;
        case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT:
            ESP_LOGI(TAG, "Advertising stopped");
            break;
        default:
            break;
    }
}

// ============================================================
// GATTS 事件回调 (static 分发)
// ============================================================

void BleJsonService::GattsEventHandler(esp_gatts_cb_event_t event,
                                       esp_gatt_if_t gatts_if,
                                       esp_ble_gatts_cb_param_t* param) {
    // 只处理属于本 App 的事件 (通过 gatts_if 或 REG 事件匹配)
    if (event == ESP_GATTS_REG_EVT) {
        if (param->reg.app_id != BLE_JSON_APP_ID) {
            return;  // 不是我们的 App 注册事件
        }
    } else {
        // 非 REG 事件: 检查 gatts_if 是否属于我们
        if (!instance_ || gatts_if != instance_->gatts_if_) {
            return;
        }
    }

    if (instance_) {
        instance_->HandleGattsEvent(event, gatts_if, param);
    }
}

// ============================================================
// HandleGattsEvent — 实例内处理
// ============================================================

void BleJsonService::HandleGattsEvent(esp_gatts_cb_event_t event,
                                      esp_gatt_if_t gatts_if,
                                      esp_ble_gatts_cb_param_t* param) {
    switch (event) {

    // ---- App 注册完成，保存 gatts_if，创建 Service ----
    case ESP_GATTS_REG_EVT:
        if (param->reg.status == ESP_GATT_OK) {
            gatts_if_ = gatts_if;
            ESP_LOGI(TAG, "GATTS app registered, gatts_if=%d", gatts_if);
            CreateService(gatts_if);
        } else {
            ESP_LOGE(TAG, "GATTS app register failed, status=%d", param->reg.status);
        }
        break;

    // ---- Service 创建完成，开始添加 Characteristic ----
    case ESP_GATTS_CREATE_EVT:
        if (param->create.status == ESP_GATT_OK) {
            service_handle_ = param->create.service_handle;
            ESP_LOGI(TAG, "Service created, handle=%d", service_handle_);

            chars_added_ = 0;

            // 1. 添加 WRITE Characteristic (App -> 设备)
            esp_gatt_char_prop_t write_prop = ESP_GATT_CHAR_PROP_BIT_WRITE;
            esp_ble_gatts_add_char(service_handle_, &write_char_uuid,
                                   ESP_GATT_PERM_WRITE,
                                   write_prop, &write_char_attr, nullptr);
        } else {
            ESP_LOGE(TAG, "Service create failed: %d", param->create.status);
        }
        break;

    // ---- Characteristic 添加完成 ----
    case ESP_GATTS_ADD_CHAR_EVT:
        if (param->add_char.status != ESP_GATT_OK) {
            ESP_LOGE(TAG, "Add char failed: uuid=0x%04x status=%d",
                     param->add_char.char_uuid.uuid.uuid16, param->add_char.status);
            break;
        }

        if (param->add_char.char_uuid.uuid.uuid16 == BLE_JSON_CHAR_WRITE_UUID) {
            write_char_handle_ = param->add_char.attr_handle;
            ESP_LOGI(TAG, "WRITE char added, handle=%d", write_char_handle_);

            // 2. 添加 NOTIFY Characteristic (设备 -> App)
            esp_gatt_char_prop_t notify_prop = ESP_GATT_CHAR_PROP_BIT_NOTIFY | ESP_GATT_CHAR_PROP_BIT_READ;
            esp_ble_gatts_add_char(service_handle_, &notify_char_uuid,
                                   ESP_GATT_PERM_READ,
                                   notify_prop, &notify_char_attr, nullptr);

        } else if (param->add_char.char_uuid.uuid.uuid16 == BLE_JSON_CHAR_NOTIFY_UUID) {
            notify_char_handle_ = param->add_char.attr_handle;
            ESP_LOGI(TAG, "NOTIFY char added, handle=%d", notify_char_handle_);

            // 为 NOTIFY char 添加 CCCD 描述符
            esp_ble_gatts_add_char_descr(service_handle_, &cccd_uuid,
                                         ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE,
                                         &cccd_attr, nullptr);

        } else if (param->add_char.char_uuid.uuid.uuid16 == BLE_JSON_CHAR_STATUS_UUID) {
            status_char_handle_ = param->add_char.attr_handle;
            ESP_LOGI(TAG, "STATUS char added, handle=%d", status_char_handle_);

            // 所有 Characteristic 添加完毕，启动 Service
            esp_ble_gatts_start_service(service_handle_);
        }
        break;

    // ---- CCCD 描述符添加完成 ----
    case ESP_GATTS_ADD_CHAR_DESCR_EVT:
        if (param->add_char_descr.status == ESP_GATT_OK) {
            notify_cccd_handle_ = param->add_char_descr.attr_handle;
            ESP_LOGI(TAG, "CCCD added, handle=%d", notify_cccd_handle_);

            // 3. 添加 STATUS Characteristic (READ only)
            esp_gatt_char_prop_t status_prop = ESP_GATT_CHAR_PROP_BIT_READ;
            esp_ble_gatts_add_char(service_handle_, &status_char_uuid,
                                   ESP_GATT_PERM_READ,
                                   status_prop, &status_char_attr, nullptr);
        } else {
            ESP_LOGE(TAG, "Add CCCD failed: %d", param->add_char_descr.status);
        }
        break;

    // ---- Service 启动完成 ----
    case ESP_GATTS_START_EVT:
        if (param->start.status == ESP_GATT_OK) {
            ESP_LOGI(TAG, "Service started");
        }
        break;

    // ---- 客户端连接 ----
    case ESP_GATTS_CONNECT_EVT: {
        conn_id_ = param->connect.conn_id;
        connected_ = true;
        notify_enabled_ = false;
        mtu_ = 23;  // 默认值，等待 MTU exchange

        ESP_LOGI(TAG, "Client connected, conn_id=%d, addr=%02x:%02x:%02x:%02x:%02x:%02x",
                 conn_id_,
                 param->connect.remote_bda[0], param->connect.remote_bda[1],
                 param->connect.remote_bda[2], param->connect.remote_bda[3],
                 param->connect.remote_bda[4], param->connect.remote_bda[5]);

        // 请求更新连接参数
        esp_ble_conn_update_params_t conn_params = {};
        memcpy(conn_params.bda, param->connect.remote_bda, sizeof(esp_bd_addr_t));
        conn_params.latency = 0;
        conn_params.max_int = 0x20;   // 40ms
        conn_params.min_int = 0x10;   // 20ms
        conn_params.timeout = 400;    // 4s
        esp_ble_gap_update_conn_params(&conn_params);

        // 连接后停止广播 (BLE 4.2 单连接时自动停止，但显式调用更安全)
        esp_ble_gap_stop_advertising();
        break;
    }

    // ---- 客户端断开 ----
    case ESP_GATTS_DISCONNECT_EVT:
        ESP_LOGI(TAG, "Client disconnected, reason=0x%x", param->disconnect.reason);
        connected_ = false;
        notify_enabled_ = false;
        mtu_ = 23;

        // 重新启动广播
        StartAdvertising();
        break;

    // ---- MTU 协商完成 ----
    case ESP_GATTS_MTU_EVT:
        mtu_ = param->mtu.mtu;
        ESP_LOGI(TAG, "MTU updated: %d", mtu_);
        break;

    // ---- WRITE 事件 ----
    case ESP_GATTS_WRITE_EVT:
        if (param->write.handle == write_char_handle_) {
            // JSON 命令数据
            ProcessWriteData(param->write.value, param->write.len);
        } else if (param->write.handle == notify_cccd_handle_ && param->write.len == 2) {
            // CCCD 写入: 开启/关闭 NOTIFY
            uint16_t cccd_value = param->write.value[0] | (param->write.value[1] << 8);
            notify_enabled_ = (cccd_value == 0x0001);
            ESP_LOGI(TAG, "NOTIFY %s", notify_enabled_ ? "enabled" : "disabled");
        }

        // 如果需要响应
        if (param->write.need_rsp) {
            esp_ble_gatts_send_response(gatts_if_, param->write.conn_id,
                                        param->write.trans_id, ESP_GATT_OK, nullptr);
        }
        break;

    // ---- READ 事件 (STATUS char) ----
    case ESP_GATTS_READ_EVT:
        // Auto response 模式下无需手动处理
        // 如果需要动态数据，可在此更新 status_char_val
        ESP_LOGD(TAG, "Read event, handle=%d", param->read.handle);
        break;

    default:
        break;
    }
}

// ============================================================
// CreateService
// ============================================================

void BleJsonService::CreateService(esp_gatt_if_t gatts_if) {
    esp_gatt_srvc_id_t service_id = {};
    service_id.is_primary = true;
    service_id.id.inst_id = 0;
    service_id.id.uuid.len = ESP_UUID_LEN_16;
    service_id.id.uuid.uuid.uuid16 = BLE_JSON_SERVICE_UUID;

    esp_ble_gatts_create_service(gatts_if, &service_id, BLE_JSON_HANDLE_NUM);
}

// ============================================================
// ProcessWriteData — 收到 WRITE 数据，放入队列
// ============================================================

void BleJsonService::ProcessWriteData(const uint8_t* data, uint16_t len) {
    if (!data || len == 0 || !cmd_queue_) return;

    // 拷贝数据到堆上 (确保 null 结尾)
    char* json_str = (char*)malloc(len + 1);
    if (!json_str) {
        ESP_LOGE(TAG, "Malloc failed for cmd data");
        return;
    }
    memcpy(json_str, data, len);
    json_str[len] = '\0';

    ESP_LOGI(TAG, "RX(%d): %s", len, json_str);

    BleJsonCmdMsg msg = {json_str, len};
    if (xQueueSend(cmd_queue_, &msg, pdMS_TO_TICKS(100)) != pdTRUE) {
        ESP_LOGW(TAG, "Cmd queue full, dropping");
        free(json_str);
    }
}

// ============================================================
// CmdProcessTask — 从队列取出 JSON 并分发到回调
// ============================================================

void BleJsonService::CmdProcessTask(void* param) {
    BleJsonService* self = static_cast<BleJsonService*>(param);
    BleJsonCmdMsg msg;

    while (true) {
        if (xQueueReceive(self->cmd_queue_, &msg, portMAX_DELAY) == pdTRUE) {
            // 解析 JSON
            cJSON* root = cJSON_Parse(msg.json_str);
            if (!root) {
                ESP_LOGW(TAG, "JSON parse failed: %s", msg.json_str);
                // 发送错误响应
                if (self->connected_ && self->notify_enabled_) {
                    self->SendResponse("error", 0, 1, "invalid json");
                }
                free(msg.json_str);
                continue;
            }

            // 提取 cmd 和 id
            cJSON* cmd_item = cJSON_GetObjectItem(root, "cmd");
            cJSON* id_item = cJSON_GetObjectItem(root, "id");
            cJSON* data_item = cJSON_GetObjectItem(root, "data");

            if (!cmd_item || !cJSON_IsString(cmd_item)) {
                ESP_LOGW(TAG, "Missing 'cmd' field");
                self->SendResponse("error", 0, 1, "missing cmd");
                cJSON_Delete(root);
                free(msg.json_str);
                continue;
            }

            std::string cmd = cmd_item->valuestring;
            int msg_id = (id_item && cJSON_IsNumber(id_item)) ? id_item->valueint : 0;

            // 分发到用户回调
            if (self->command_callback_) {
                self->command_callback_(cmd, msg_id, data_item);
            } else {
                ESP_LOGW(TAG, "No command callback, cmd=%s ignored", cmd.c_str());
                self->SendResponse(cmd, msg_id, 2, "no handler");
            }

            cJSON_Delete(root);
            free(msg.json_str);
        }
    }
}