GrabBag/Module/BinocularMarkReceiver/Src/BinocularMarkReceiver.cpp

#include "BinocularMarkReceiver.h"
#include "json/json.h"
#include "VrLog.h"
#include <algorithm>
#include <chrono>
#include <cstring>


using namespace  VA;

// 静态工厂方法实现
int IBinocularMarkReceiver::CreateInstance(IBinocularMarkReceiver** ppReceiver)
{
    if (!ppReceiver) {
        return -1;
    }

    *ppReceiver = new BinocularMarkReceiver();
    return 0;
}

BinocularMarkReceiver::BinocularMarkReceiver()
    : m_pTcpClient(nullptr)
    , m_serverPort(0)
    , m_bConnected(false)
    , m_bSingleDetectionReady(false)
    , m_bImageDataReady(false)
    , m_bLeftCameraInfoReady(false)
    , m_bRightCameraInfoReady(false)
    , m_bCalibrationMatrixReady(false)
    , m_bHeartbeatRunning(false)
    , m_bHeartbeatEnabled(false)
    , m_nHeartbeatInterval(30)
{
    m_pTcpClient = IVrTCPClient::CreateInstance();
}

BinocularMarkReceiver::~BinocularMarkReceiver()
{
    Disconnect();

    if (m_pTcpClient) {
        IVrTCPClient::DestroyInstance(m_pTcpClient);
        m_pTcpClient = nullptr;
    }
}

int BinocularMarkReceiver::Connect(const std::string& serverIp, uint16_t serverPort)
{
    if (m_bConnected) {
        LOG_DEBUG("Already connected\n");
        return 0;
    }

    m_serverIp = serverIp;
    m_serverPort = serverPort;

    LOG_DEBUG("Connecting to %s:%d\n", serverIp.c_str(), serverPort);

    // 连接到服务器
    int ret = m_pTcpClient->LinkDevice(serverIp, serverPort, false, linkEventCallback, this);
    if (ret != 0) {
        LOG_DEBUG("LinkDevice failed: %d\n", ret);
        return -1;
    }

    // 启动工作线程
    ret = m_pTcpClient->StartWork(tcpRecvCallback, this);
    if (ret != 0) {
        LOG_DEBUG("StartWork failed: %d\n", ret);
        m_pTcpClient->CloseDevice();
        return -1;
    }

    LOG_DEBUG("Connect initiated\n");
    return 0;
}

int BinocularMarkReceiver::Disconnect()
{
    if (!m_bConnected) {
        return 0;
    }

    // 停止心跳线程
    m_bHeartbeatRunning = false;
    m_cvHeartbeat.notify_one();  // 唤醒心跳线程
    if (m_heartbeatThread.joinable()) {
        m_heartbeatThread.join();
    }

    // 断开连接
    if (m_pTcpClient) {
        m_pTcpClient->CloseDevice();
    }

    m_bConnected = false;
    m_dataBuffer.clear();

    // 通知所有等待的条件变量，避免其他线程卡住
    m_cvSingleDetection.notify_all();
    m_cvImageData.notify_all();
    m_cvLeftCameraInfo.notify_all();
    m_cvRightCameraInfo.notify_all();
    m_cvCalibrationMatrix.notify_all();

    return 0;
}

bool BinocularMarkReceiver::IsConnected() const
{
    return m_bConnected;
}

void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_markResultCallback = callback;
}

void BinocularMarkReceiver::SetImageCallback(ImageCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_imageCallback = callback;
}

void BinocularMarkReceiver::SetEventCallback(EventCallback callback)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_eventCallback = callback;
}

// TCP连接状态回调
void BinocularMarkReceiver::linkEventCallback(IVrTCPClient* pClient, bool connected, void* pParam)
{
    BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
    if (!pThis) return;

    pThis->m_bConnected = connected;

    // 获取回调函数的拷贝，在锁外调用
    EventCallback callback;
    {
        std::lock_guard<std::mutex> lock(pThis->m_mutex);
        callback = pThis->m_eventCallback;
    }

    if (connected) {
        LOG_DEBUG("Connected to server\n");
        // 在锁外触发连接事件
        if (callback) {
            callback(ReceiverEventType::CONNECTED, "");
        }

        // 启动心跳线程
        pThis->m_bHeartbeatRunning = true;
        pThis->m_heartbeatThread = std::thread(&BinocularMarkReceiver::heartbeatThreadFunc, pThis);
    } else {
        LOG_DEBUG("Disconnected from server\n");
        // 在锁外触发断开连接事件
        if (callback) {
            callback(ReceiverEventType::DISCONNECTED, "");
        }
    }
}

// TCP数据接收回调
void BinocularMarkReceiver::tcpRecvCallback(IVrTCPClient* pClient, const char* pData, const int nLen, void* pParam)
{
    BinocularMarkReceiver* pThis = static_cast<BinocularMarkReceiver*>(pParam);
    if (!pThis || !pData || nLen <= 0) return;

    // 追加到缓冲区
    {
        std::lock_guard<std::mutex> lock(pThis->m_mutex);
        pThis->m_dataBuffer.insert(pThis->m_dataBuffer.end(), pData, pData + nLen);
    }

    // 解析数据帧
    pThis->parseFrames();
}

// 心跳线程函数
void BinocularMarkReceiver::heartbeatThreadFunc()
{
    while (m_bHeartbeatRunning) {
        // 使用 condition_variable 实现可中断的等待
        std::unique_lock<std::mutex> lock(m_mutexHeartbeat);
        m_cvHeartbeat.wait_for(lock, std::chrono::seconds(m_nHeartbeatInterval),
                               [this] { return !m_bHeartbeatRunning; });

        if (!m_bHeartbeatRunning) break;

        // 检查是否启用心跳
        if (!m_bHeartbeatEnabled) continue;

        // 发送心跳消息
        Json::Value root;
        root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::system_clock::now().time_since_epoch()).count());

        Json::FastWriter writer;
        std::string jsonData = writer.write(root);

        sendJsonMessage("heartbeat", jsonData);
    }
}

void BinocularMarkReceiver::parseFrames()
{
    std::vector<std::string> messagesToProcess;

    {
        std::lock_guard<std::mutex> lock(m_mutex);

        while (true) {
            // 查找帧头
            auto it = std::search(m_dataBuffer.begin(), m_dataBuffer.end(), FRAME_HEADER, FRAME_HEADER + FRAME_HEADER_SIZE);
            if (it == m_dataBuffer.end()) {
                m_dataBuffer.clear();
                break;
            }

            // 丢弃帧头之前的数据
            if (it != m_dataBuffer.begin()) {
                m_dataBuffer.erase(m_dataBuffer.begin(), it);
            }

            // 检查是否有足够的数据来读取长度字段
            if (m_dataBuffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE) {
                break;
            }

            // 读取数据长度（8字节ASCII字符串格式）
            std::string lengthStr(m_dataBuffer.begin() + FRAME_HEADER_SIZE, m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE);
            int64_t dataLength = std::stoll(lengthStr);

            // 检查数据长度是否合理（最大50MB，支持双目高分辨率图像）
            if (dataLength < 0 || dataLength > 50 * 1024 * 1024) {
                LOG_WARN("Invalid data length: %lld, discarding frame header\n", dataLength);
                m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
                continue;
            }

            // 计算完整帧的总长度
            size_t totalFrameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;

            // 检查是否有完整的帧
            if (m_dataBuffer.size() < totalFrameLength) {
                break;
            }

            // 提取JSON数据
            std::string jsonData(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE,
                                m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength);

            // 验证帧尾
            std::string tail(m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength,
                            m_dataBuffer.begin() + FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE);
            if (tail != FRAME_TAIL) {
                m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + FRAME_HEADER_SIZE);
                continue;
            }

            // 收集消息，稍后在锁外处理
            messagesToProcess.push_back(jsonData);

            // 移除已处理的帧
            m_dataBuffer.erase(m_dataBuffer.begin(), m_dataBuffer.begin() + totalFrameLength);
        }
    } // 释放锁

    // 在锁外处理所有消息
    for (const auto& jsonData : messagesToProcess) {
        handleJsonMessage(jsonData);
    }
}

void BinocularMarkReceiver::handleJsonMessage(const std::string& jsonData)
{
    Json::Reader reader;
    Json::Value root;

    if (!reader.parse(jsonData, root)) {
        LOG_DEBUG("JSON parse failed\n");
        return;
    }

    if (!root.isObject()) {
        LOG_DEBUG("JSON is not an object\n");
        return;
    }

    // 获取消息类型
    std::string messageType = root.get("msg_type", "").asString();
    LOG_DEBUG("Received message type: %s\n", messageType.c_str());

    if (messageType == "mark_result") {
        handleMarkResult(jsonData);
    } else if (messageType == "single_detection_result") {
        handleSingleDetectionResult(jsonData);
    } else if (messageType == "image_data") {
        handleImageData(jsonData);
    } else if (messageType == "heartbeat") {
        handleHeartbeat(jsonData);
    } else if (messageType == "heartbeat_ack") {
        handleHeartbeatAck(jsonData);
    } else if (messageType == "cmd_response") {
        handleCommandResponse(jsonData);
    } else if (messageType == "camera_info_response") {
        handleCameraInfoResponse(jsonData);
    } else if (messageType == "calibration_matrix_response") {
        handleCalibrationMatrixResponse(jsonData);
    } else {
        LOG_DEBUG("Unknown message type: %s\n", messageType.c_str());
    }
}

void BinocularMarkReceiver::handleMarkResult(const std::string& jsonStr)
{
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) return;

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    int errorCode = root.get("error_code", -1).asInt();

    std::vector<VrMark3D> marks;

    if (errorCode == 0 && root.isMember("marks")) {
        const Json::Value& marksArray = root["marks"];
        for (unsigned int i = 0; i < marksArray.size(); i++) {
            VrMark3D mark;
            mark.markID = marksArray[i].get("mark_id", -1).asInt();
            mark.x = marksArray[i].get("x", 0.0).asDouble();
            mark.y = marksArray[i].get("y", 0.0).asDouble();
            mark.z = marksArray[i].get("z", 0.0).asDouble();
            marks.push_back(mark);
        }
    }

    // 提取图像数据
    std::string leftImageBase64 = root.get("left_image", "").asString();
    std::string rightImageBase64 = root.get("right_image", "").asString();

    // 如果有图像数据且注册了图像回调，触发图像回调
    if (!leftImageBase64.empty() && !rightImageBase64.empty()) {
        ImageCallback imageCallback;
        {
            std::lock_guard<std::mutex> lock(m_mutex);
            imageCallback = m_imageCallback;
        }
        if (imageCallback) {
            imageCallback(leftImageBase64, rightImageBase64, timestamp);
        }
    }

    // 如果有marks数据且注册了mark回调，触发mark回调
    if (!marks.empty()) {
        MarkResultCallback callback;
        {
            std::lock_guard<std::mutex> lock(m_mutex);
            callback = m_markResultCallback;
        }
        if (callback) {
            callback(marks, timestamp, errorCode);
        }
    }
}

void BinocularMarkReceiver::handleHeartbeat(const std::string& jsonStr)
{
    // 收到服务器心跳，回复心跳应答
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());

    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    sendJsonMessage("heartbeat_ack", jsonData);
}

void BinocularMarkReceiver::handleHeartbeatAck(const std::string& jsonStr)
{
    // 心跳应答，暂时不处理
}

void BinocularMarkReceiver::handleCommandResponse(const std::string& jsonStr)
{
    // 命令应答，暂时不处理
}

std::string BinocularMarkReceiver::buildFrame(const std::string& jsonData)
{
    std::string frame;

    // 添加帧头
    frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);

    // 添加数据长度（8字节ASCII字符串格式）
    char lengthStr[9];
    snprintf(lengthStr, sizeof(lengthStr), "%08zu", jsonData.size());
    frame.append(lengthStr, FRAME_LENGTH_SIZE);

    // 添加JSON数据
    frame.append(jsonData);

    // 添加帧尾
    frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);

    return frame;
}

int BinocularMarkReceiver::sendJsonMessage(const std::string& messageType, const std::string& jsonData)
{
    if (!m_bConnected) {
        return -1;
    }

    // 解析JSON并添加msg_type字段
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonData, root)) {
        root = Json::Value(Json::objectValue);
    }
    root["msg_type"] = messageType;

    // 转换为JSON字符串
    Json::FastWriter writer;
    std::string finalJsonData = writer.write(root);

    // 构造数据帧
    std::string frame = buildFrame(finalJsonData);

    // 发送数据
    if (!m_pTcpClient->SendData(frame.c_str(), frame.size())) {
        return -1;
    }

    return 0;
}

// ========== 新增接口实现 ==========

IBinocularMarkReceiver::SingleDetectionResult BinocularMarkReceiver::RequestSingleDetection(int timeoutMs)
{
    SingleDetectionResult result;
    result.errorCode = -1;

    if (!m_bConnected) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    m_bSingleDetectionReady = false;
    if (sendJsonMessage("cmd_single_detection", jsonData) != 0) {
        return result;
    }

    // 使用condition_variable同步等待结果
    std::unique_lock<std::mutex> lock(m_mutex);
    if (m_cvSingleDetection.wait_for(lock, std::chrono::milliseconds(timeoutMs),
                                     [this] { return m_bSingleDetectionReady; })) {
        return m_pendingSingleDetectionResult;
    }

    result.errorCode = -2; // 超时
    return result;
}

IBinocularMarkReceiver::ImageData BinocularMarkReceiver::RequestSingleImage(int timeoutMs)
{
    ImageData result;
    result.timestamp = 0;

    if (!m_bConnected) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    m_bImageDataReady = false;
    if (sendJsonMessage("cmd_single_image", jsonData) != 0) {
        return result;
    }

    // 使用condition_variable同步等待结果
    std::unique_lock<std::mutex> lock(m_mutex);
    if (m_cvImageData.wait_for(lock, std::chrono::milliseconds(timeoutMs), [this] { return m_bImageDataReady; })) {
        return m_pendingImageData;
    }

    return result;
}

int BinocularMarkReceiver::StartWork()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_start_work", jsonData);
}

int BinocularMarkReceiver::StopWork()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_stop_work", jsonData);
}

int BinocularMarkReceiver::StartCapture()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_start_continuous_image", jsonData);
}

int BinocularMarkReceiver::StopCapture()
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_stop_continuous_image", jsonData);
}

int BinocularMarkReceiver::SetCalibrationMatrix(const std::string& calibrationXml)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["calibration_xml"] = calibrationXml;
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_calibration", jsonData);
}

int BinocularMarkReceiver::SetExposureTime(SVrCameraEnum target, double exposureTime)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["exposure_time"] = exposureTime;
    if (target == SVrCameraEnum::LEFT) {
        root["camera"] = "left";
    } else if (target == SVrCameraEnum::RIGHT) {
        root["camera"] = "right";
    }
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_exposure_time", jsonData);
}

int BinocularMarkReceiver::SetGain(SVrCameraEnum target, double gain)
{
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["gain"] = gain;
    if (target == SVrCameraEnum::LEFT) {
        root["camera"] = "left";
    } else if (target == SVrCameraEnum::RIGHT) {
        root["camera"] = "right";
    }
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);
    return sendJsonMessage("cmd_set_gain", jsonData);
}

IBinocularMarkReceiver::CameraInfo BinocularMarkReceiver::GetCameraInfo(SVrCameraEnum target, int timeoutMs)
{
    CameraInfo result;
    result.exposureTime = 0.0;
    result.gain = 0.0;

    if (!m_bConnected || target == SVrCameraEnum::BOTH) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    root["camera"] = (target == SVrCameraEnum::LEFT) ? "left" : "right";
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    if (target == SVrCameraEnum::LEFT) {
        m_bLeftCameraInfoReady = false;
        if (sendJsonMessage("cmd_get_camera_info", jsonData) != 0) {
            return result;
        }
        std::unique_lock<std::mutex> lock(m_mutex);
        if (m_cvLeftCameraInfo.wait_for(lock, std::chrono::milliseconds(timeoutMs),
                                         [this] { return m_bLeftCameraInfoReady; })) {
            return m_pendingLeftCameraInfo;
        }
    } else {
        m_bRightCameraInfoReady = false;
        if (sendJsonMessage("cmd_get_camera_info", jsonData) != 0) {
            return result;
        }
        std::unique_lock<std::mutex> lock(m_mutex);
        if (m_cvRightCameraInfo.wait_for(lock, std::chrono::milliseconds(timeoutMs),
                                          [this] { return m_bRightCameraInfoReady; })) {
            return m_pendingRightCameraInfo;
        }
    }

    return result;
}

std::string BinocularMarkReceiver::GetCalibrationMatrix(int timeoutMs)
{
    std::string result;

    if (!m_bConnected) {
        return result;
    }

    // 发送请求
    Json::Value root;
    root["timestamp"] = static_cast<Json::Int64>(std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count());
    Json::FastWriter writer;
    std::string jsonData = writer.write(root);

    m_bCalibrationMatrixReady = false;
    if (sendJsonMessage("cmd_get_calibration", jsonData) != 0) {
        return result;
    }

    std::unique_lock<std::mutex> lock(m_mutex);
    if (m_cvCalibrationMatrix.wait_for(lock, std::chrono::milliseconds(timeoutMs),
                                        [this] { return m_bCalibrationMatrixReady; })) {
        return m_pendingCalibrationMatrix;
    }

    return result;
}

void BinocularMarkReceiver::handleSingleDetectionResult(const std::string& jsonStr)
{
    LOG_DEBUG("handleSingleDetectionResult start, JSON size: %zu\n", jsonStr.size());

    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        LOG_DEBUG("handleSingleDetectionResult JSON parse failed\n");
        return;
    }

    LOG_DEBUG("handleSingleDetectionResult JSON parsed\n");

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    int errorCode = root.get("error_code", -1).asInt();
    std::string leftImageBase64 = root.get("left_image", "").asString();
    std::string rightImageBase64 = root.get("right_image", "").asString();

    LOG_DEBUG("handleSingleDetectionResult got data: timestamp=%lld, error=%d, left_size=%zu, right_size=%zu\n",
              timestamp, errorCode, leftImageBase64.size(), rightImageBase64.size());

    std::vector<VrMark3D> marks;
    if (errorCode == 0 && root.isMember("marks")) {
        const Json::Value& marksArray = root["marks"];
        for (unsigned int i = 0; i < marksArray.size(); i++) {
            VrMark3D mark;
            mark.markID = marksArray[i].get("mark_id", -1).asInt();
            mark.x = marksArray[i].get("x", 0.0).asDouble();
            mark.y = marksArray[i].get("y", 0.0).asDouble();
            mark.z = marksArray[i].get("z", 0.0).asDouble();
            marks.push_back(mark);
        }
    }

    LOG_DEBUG("handleSingleDetectionResult parsed %zu marks\n", marks.size());

    // 存储结果并通知等待的线程
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        LOG_DEBUG("handleSingleDetectionResult storing data\n");
        m_pendingSingleDetectionResult.marks = marks;
        m_pendingSingleDetectionResult.leftImageBase64 = leftImageBase64;
        m_pendingSingleDetectionResult.rightImageBase64 = rightImageBase64;
        m_pendingSingleDetectionResult.timestamp = timestamp;
        m_pendingSingleDetectionResult.errorCode = errorCode;
        m_bSingleDetectionReady = true;
        LOG_DEBUG("handleSingleDetectionResult data stored\n");
    } // 释放锁后再通知

    LOG_DEBUG("handleSingleDetectionResult notifying\n");
    m_cvSingleDetection.notify_one();
    LOG_DEBUG("handleSingleDetectionResult complete\n");
}

void BinocularMarkReceiver::handleImageData(const std::string& jsonStr)
{
    LOG_DEBUG("handleImageData start, JSON size: %zu\n", jsonStr.size());

    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        LOG_DEBUG("handleImageData JSON parse failed\n");
        return;
    }

    LOG_DEBUG("handleImageData JSON parsed\n");

    int64_t timestamp = root.get("timestamp", 0).asInt64();
    std::string leftImageBase64 = root.get("left_image", "").asString();
    std::string rightImageBase64 = root.get("right_image", "").asString();

    LOG_DEBUG("handleImageData got data: timestamp=%lld, left_size=%zu, right_size=%zu\n",  timestamp, leftImageBase64.size(), rightImageBase64.size());

    // 存储结果并通知等待的线程
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        LOG_DEBUG("handleImageData storing data\n");
        m_pendingImageData.leftImageBase64 = leftImageBase64;
        m_pendingImageData.rightImageBase64 = rightImageBase64;
        m_pendingImageData.timestamp = timestamp;
        m_bImageDataReady = true;
        LOG_DEBUG("handleImageData data stored\n");
    } // 释放锁后再通知

    LOG_DEBUG("handleImageData notifying\n");
    m_cvImageData.notify_one();
    LOG_DEBUG("handleImageData complete\n");
}

void BinocularMarkReceiver::handleCameraInfoResponse(const std::string& jsonStr)
{
    LOG_DEBUG("handleCameraInfoResponse start, JSON size: %zu\n", jsonStr.size());

    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        LOG_DEBUG("handleCameraInfoResponse JSON parse failed\n");
        return;
    }

    LOG_DEBUG("handleCameraInfoResponse JSON parsed\n");

    std::string camera = root.get("camera", "").asString();
    std::string serialNumber = root.get("serial_number", "").asString();
    std::string modelName = root.get("model_name", "").asString();
    std::string displayName = root.get("display_name", "").asString();
    double exposureTime = root.get("exposure_time", 0.0).asDouble();
    double gain = root.get("gain", 0.0).asDouble();

    LOG_DEBUG("handleCameraInfoResponse got data: camera=%s, SN=%s, model=%s, exposure=%.2f, gain=%.2f\n",
              camera.c_str(), serialNumber.c_str(), modelName.c_str(), exposureTime, gain);

    // 存储结果并通知等待的线程
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        LOG_DEBUG("handleCameraInfoResponse storing data\n");

        if (camera == "left") {
            m_pendingLeftCameraInfo.serialNumber = serialNumber;
            m_pendingLeftCameraInfo.modelName = modelName;
            m_pendingLeftCameraInfo.displayName = displayName;
            m_pendingLeftCameraInfo.exposureTime = exposureTime;
            m_pendingLeftCameraInfo.gain = gain;
            m_bLeftCameraInfoReady = true;
            LOG_DEBUG("handleCameraInfoResponse left camera data stored\n");
        } else if (camera == "right") {
            m_pendingRightCameraInfo.serialNumber = serialNumber;
            m_pendingRightCameraInfo.modelName = modelName;
            m_pendingRightCameraInfo.displayName = displayName;
            m_pendingRightCameraInfo.exposureTime = exposureTime;
            m_pendingRightCameraInfo.gain = gain;
            m_bRightCameraInfoReady = true;
            LOG_DEBUG("handleCameraInfoResponse right camera data stored\n");
        }
    } // 释放锁后再通知

    LOG_DEBUG("handleCameraInfoResponse notifying\n");
    if (camera == "left") {
        m_cvLeftCameraInfo.notify_one();
    } else if (camera == "right") {
        m_cvRightCameraInfo.notify_one();
    }
    LOG_DEBUG("handleCameraInfoResponse complete\n");
}

void BinocularMarkReceiver::handleCalibrationMatrixResponse(const std::string& jsonStr)
{
    Json::Reader reader;
    Json::Value root;
    if (!reader.parse(jsonStr, root)) {
        return;
    }

    std::string calibrationXml = root.get("calibration_xml", "").asString();

    {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_pendingCalibrationMatrix = calibrationXml;
        m_bCalibrationMatrixReady = true;
    }

    m_cvCalibrationMatrix.notify_one();
}