#include "BeltTearingPresenter.h"
#include "PathManager.h"
#include "version.h"
#include "PointCloudImageUtils.h"
#include <QUuid>
#include <QDataStream>
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include <mutex>
#include <deque>
#include <QDateTime>
#include <QBuffer>
#include <QMutexLocker>
#include <QFile>
#include <QDir>
#include <iostream>
#include <thread>
#include <future>
#include <QPainter>
#include <QPainterPath>
#include <cmath>

#include "VrLog.h"
#include "VrSimpleLog.h"
#include "VrTimeUtils.h"
#include <algorithm>

// 静态实例指针
BeltTearingPresenter* BeltTearingPresenter::s_instance = nullptr;

BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
    : QObject(parent)
    , m_tcpServer(nullptr)
    , m_config(nullptr)
    , m_eyeDevice(nullptr)
    , m_cameraInitTimer(new QTimer(this))
    , m_cameraInitialized(false)
    , m_cameraDetecting(false)
    , m_lineCounter(0)
    , m_bAlgoDetectThreadRunning(false)
    , m_pRobotProtocol(nullptr)   // 初始化RobotProtocol指针
    , m_pModbusRTUMaster(nullptr) // 初始化ModbusRTUMaster指针
    , m_bRobotConnected(false)   // 初始化连接状态
{
    // 设置静态实例
    s_instance = this;

    QString versionWithBuildTime = QString("%1.%2_%3%4%5%6%7%8")
        .arg(BELT_TEARING_SERVER_VERSION_STRING)
        .arg(BELT_TEARING_SERVER_VERSION_BUILD)
        .arg(YEAR)
        .arg(MONTH, 2, 10, QChar('0'))
        .arg(DAY, 2, 10, QChar('0'))
        .arg(HOUR, 2, 10, QChar('0'))
        .arg(MINUTE, 2, 10, QChar('0'))
        .arg(SECOND, 2, 10, QChar('0'));
    // 打印版本信息
    LOG_INFO("Initializing %s\n", versionWithBuildTime.toStdString().c_str());

    // 连接定时器信号
    connect(m_cameraInitTimer, &QTimer::timeout, this, &BeltTearingPresenter::onCameraInitTimer);

    // 启动算法检测线程
    m_bAlgoDetectThreadRunning = true;
    m_algoDetectThread = std::thread(&BeltTearingPresenter::_AlgoDetectThread, this);
    m_algoDetectThread.detach();

    // 创建TCP服务器实例
    if (!VrCreatYTCPServer(&m_tcpServer)) {
        LOG_ERROR("Failed to create TCP server\n");
        m_tcpServer = nullptr;
    }

    // 创建配置实例
    IVrBeltTearingConfig::CreateInstance(&m_config);
    if (m_config) {
        m_config->SetConfigChangeNotify(this);
    }

    // 初始化SDK算法参数 - 使用配置系统的默认值
    m_algorithmParam = configToSDKParam(BeltTearingConfigResult());

    // 初始化相机
    initializeCamera();
    
    // 初始化机械臂协议
    int nRet = InitRobotProtocol();
    if (nRet != 0) {
        LOG_WARNING("Robot protocol initialization failed\n");
        m_bRobotConnected = false;
    } else {
        LOG_INFO("Robot protocol initialization successful\n");
        m_bRobotConnected = true;
    }
}

BeltTearingPresenter::~BeltTearingPresenter()
{
    // 清除静态实例
    s_instance = nullptr;

    stopServer();
    stopCamera();

    // 停止算法检测线程
    m_bAlgoDetectThreadRunning = false;
    m_algoDetectCondition.notify_all();
    
    // 等待算法检测线程结束
    if (m_algoDetectThread.joinable()) {
        m_algoDetectThread.join();
    }
    
    // 清理激光线队列中的内存
    std::lock_guard<std::mutex> lock(m_queueMutex);
    for (auto& line : m_laserLineQueue) {
        if (line.p3DPoint) {
            free(line.p3DPoint);
            line.p3DPoint = nullptr;
        }
    }
    m_laserLineQueue.clear();

    if (m_tcpServer) {
        delete m_tcpServer;
        m_tcpServer = nullptr;
    }

    if (m_config) {
        delete m_config;
        m_config = nullptr;
    }
    
    // 释放RobotProtocol资源
    if (m_pRobotProtocol) {
        m_pRobotProtocol->Deinitialize();
        delete m_pRobotProtocol;
        m_pRobotProtocol = nullptr;
    }
    
    // 释放ModbusRTUMaster资源
    if (m_pModbusRTUMaster) {
        m_pModbusRTUMaster->Deinitialize();
        delete m_pModbusRTUMaster;
        m_pModbusRTUMaster = nullptr;
    }
}

bool BeltTearingPresenter::loadConfiguration(const QString& configFilePath)
{
    if (!m_config) {
        LOG_WARNING("Config instance not created\n");
        return false;
    }
    
    m_configFilePath = configFilePath;
    
    try {
        // 加载配置文件
        m_configResult = m_config->LoadConfig(configFilePath.toStdString());
        
        LOG_INFO("Configuration loaded successfully:\n");
        LOG_INFO("  Server Port: %d\n", m_configResult.serverPort);
        LOG_INFO("  Project Type: %d\n", static_cast<int>(m_configResult.projectType));
        LOG_INFO("  Servers count: %d\n", m_configResult.servers.size());
        
        // 应用算法参数
        applyAlgorithmParameters(m_configResult.algorithmParams);
        
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Error loading configuration: %s\n", e.what());
        return false;
    }
}

void BeltTearingPresenter::applyAlgorithmParameters(const BeltTearingAlgorithmParams& params)
{
    // 使用配置结构转换为SDK参数
    BeltTearingConfigResult tempConfig;
    tempConfig.algorithmParams = params;
    m_algorithmParam = configToSDKParam(tempConfig);

    // 应用算法参数
    LOG_DEBUG("Applying SDK algorithm parameters...\n");
    LOG_DEBUG("  Scan X Scale: %f\n", m_algorithmParam.scanXScale);
    LOG_DEBUG("  Scan Y Scale: %f\n", m_algorithmParam.scanYScale);
    LOG_DEBUG("  Difference Bin Threshold: %f\n", m_algorithmParam.differnceBinTh);
    LOG_DEBUG("  Min Tear Length: %f\n", m_algorithmParam.tearingMinLen);
    LOG_DEBUG("  Min Tear Gap: %f\n", m_algorithmParam.tearingMinGap);
    LOG_DEBUG("  Same Gap Threshold: %f\n", m_algorithmParam.extractPara.sameGapTh);
    LOG_DEBUG("  Gap Check Window: %d\n", m_algorithmParam.extractPara.gapChkWin);

    // 监控参数
    LOG_DEBUG("  Check Interval: %d ms\n", params.monitoringParam.checkInterval);
    LOG_DEBUG("  Alert Threshold: %f\n", params.monitoringParam.alertThreshold);

    // 调试参数
    if (m_configResult.debugParam.enableDebug) {
        LOG_DEBUG("Debug mode enabled:\n");
        LOG_DEBUG("  Save debug images: %s\n", (m_configResult.debugParam.saveDebugImage ? "Yes" : "No"));
        LOG_DEBUG("  Print detail log: %s\n", (m_configResult.debugParam.printDetailLog ? "Yes" : "No"));
    }
}

void BeltTearingPresenter::OnConfigChanged(const BeltTearingConfigResult& configResult)
{
    LOG_INFO("Configuration changed notification received\n");
    
    // 更新配置结果
    m_configResult = configResult;
    
    // 重新应用算法参数
    applyAlgorithmParameters(configResult.algorithmParams);
    
    // 如果服务器端口改变，可能需要重启服务器
    LOG_INFO("Server port changed, restarting server...\n");
    stopServer();
    startServer(configResult.serverPort);
}

void BeltTearingPresenter::sendTestData(std::string fileName){
    if (!m_tcpServer) {
        LOG_WARNING("TCP server not initialized, cannot send test data\n");
        return;
    }

    if(m_pRobotProtocol){
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
    }
    
    // 判断文件类型
    std::ifstream inputFile(fileName);
    if (!inputFile.is_open()) {
        LOG_WARN("UN open file \n");
        return;
    } else {
        LOG_DEBUG("------------------------ \n");
    }

    std::string line;
    std::vector<SVzNL3DPosition> sVzNLPostion;
    sVzNLPostion.clear();

    int nIndex = 0;
    SVzLaserLineData pLaserLine;
    
    while (std::getline(inputFile, line)) {
        if (line.find("Line_") == 0) {

            if(!sVzNLPostion.empty()){
                pLaserLine.p3DPoint = sVzNLPostion.data();
                pLaserLine.nPointCount = sVzNLPostion.size();
                addLaserLineToQueue(&pLaserLine);
            }
            sscanf(line.c_str(), "Line_%lld_%lld", &pLaserLine.llFrameIdx, &pLaserLine.llTimeStamp);
            sVzNLPostion.clear();

            nIndex++;

        } else if (line.find("{") == 0) {
            float lx, ly, rx, ry;
            SVzNL3DPosition pos;
            sscanf(line.c_str(), "{ %lf, %lf, %lf }-{ %f, %f}-{ %f, %f }",
                    &pos.pt3D.x, &pos.pt3D.y, &pos.pt3D.z, &lx, &ly, &rx, &ry);

            sVzNLPostion.push_back(pos);
        }
    }

    inputFile.close();
}

// 发送模拟撕裂结果数据
void BeltTearingPresenter::sendSimulationData()
{
    LOG_INFO("Sending simulation data\n");

    if(m_pRobotProtocol){
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
    }
    
    // 创建模拟的撕裂结果数据
    std::vector<SSG_beltTearingInfo> simulationResults;
    
    // 创建第一个模拟撕裂结果
    SSG_beltTearingInfo tear1;
    tear1.tearID = 11;
    tear1.tearStatus = keSG_tearStatus_New;  // 假设1表示有效撕裂
    tear1.tearWidth = 15.5f;  // 撕裂宽度 15.5mm
    tear1.tearDepth = 8.2f;   // 撕裂深度 8.2mm
    // 其他字段由于结构定义不明确，暂时不填充
    
    simulationResults.push_back(tear1);
    
    // 创建第二个模拟撕裂结果
    SSG_beltTearingInfo tear2;
    tear2.tearID = 12;
    tear2.tearStatus = keSG_tearStatus_Growing;  // 假设2表示无效撕裂
    tear2.tearWidth = 22.3f;  // 撕裂宽度 22.3mm
    tear2.tearDepth = 12.7f;  // 撕裂深度 12.7mm
    // 其他字段由于结构定义不明确，暂时不填充
    
    simulationResults.push_back(tear2);
    
    // 发送撕裂结果到所有客户端
    sendTearingResults(simulationResults);
    SendDetectionResultToRobot(simulationResults);  // 发送检测结果到机械臂
    
    LOG_INFO("Simulation tearing data sent successfully\n");
}

// 发送模拟图像数据
void BeltTearingPresenter::sendSimulationImageData()
{
    LOG_INFO("Sending simulation image data\n");

    // 创建一个模拟的图像
    QImage simulationImage(800, 600, QImage::Format_RGB32);
    
    // 填充背景色
    simulationImage.fill(Qt::white);
    
    // 创建 QPainter 对象用于绘制
    QPainter painter(&simulationImage);
    
    // 发送图像到所有客户端
    sendImageToClients(simulationImage);
    
    LOG_INFO("Simulation image data sent successfully\n");
}

bool BeltTearingPresenter::initializeCamera()
{
    if (m_eyeDevice) {
        return true; // 已经初始化过
    }
    
    int result = IVrEyeDevice::CreateObject(&m_eyeDevice);
    if (result != 0 || !m_eyeDevice) {
        LOG_ERROR("Failed to create VrEyeDevice object, error code: %d\n", result);
        return false;
    }
    
    result = m_eyeDevice->InitDevice();
    if (result != 0) {
        LOG_ERROR("Failed to initialize VrEyeDevice, error code: %d\n", result);
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
        return false;
    }
    
    // 设置状态回调
    result = m_eyeDevice->SetStatusCallback(OnStatusCallback, this);
    if (result != 0) {
        LOG_WARNING("Failed to set status callback, error code: %d\n", result);
    }
    
    LOG_INFO("VrEyeDevice initialized successfully\n");
    return true;
}

void BeltTearingPresenter::startCamera()
{
    if (!initializeCamera()) {
        // 启动定时器持续重试初始化
        LOG_WARNING("Camera initialization failed, starting retry timer...\n");
        m_cameraInitTimer->start(5000); // 每5秒重试一次
        return;
    }

    // 准备相机IP参数（使用配置中的第一个相机）
    const char* pCameraIP = nullptr;
    if (!m_configResult.cameras.empty() && !m_configResult.cameras[0].cameraIP.empty()) {
        pCameraIP = m_configResult.cameras[0].cameraIP.c_str();
        LOG_INFO("Using configured camera [%s] IP: %s\n",
                 m_configResult.cameras[0].name.c_str(), pCameraIP);
    } else {
        LOG_INFO("No camera IP configured, using auto-discovery\n");
    }

    // 尝试打开设备
    int result = m_eyeDevice->OpenDevice(pCameraIP, false, false, true);
    if (result != 0) {
        LOG_WARNING("Failed to open camera device, error code: %d, retrying...\n", result);

        // 启动定时器持续重试连接
        m_cameraInitTimer->start(3000); // 每3秒重试一次
        return;
    }

    LOG_INFO("Camera opened successfully\n");

    // 停止重试定时器
    m_cameraInitTimer->stop();
    m_cameraInitialized = true;

    // 开始检测
    result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
    LOG_DEBUG("Camera detection started, result: %d\n", result);
    if (result != 0) {
        LOG_ERROR("Failed to start detection, error code: %d\n", result);
        return;
    }

    m_cameraDetecting = true;

    // 设置机械臂工作状态为工作中
    if (m_pRobotProtocol) {
        int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
        if (robotResult != 0) {
            LOG_WARNING("Failed to set robot work status to working, error code: %d\n", robotResult);
        } else {
            LOG_DEBUG("Robot work status set to working\n");
        }
    }
}

void BeltTearingPresenter::stopCamera()
{
    m_cameraInitTimer->stop();
    
    if (m_eyeDevice) {
        if (m_cameraDetecting) {
            m_eyeDevice->StopDetect();
            m_cameraDetecting = false;
        }
        
        if (m_cameraInitialized) {
            m_eyeDevice->CloseDevice();
            m_cameraInitialized = false;
        }
        
        delete m_eyeDevice;
        m_eyeDevice = nullptr;
    }
    
    // 设置机械臂工作状态为空闲
    if (m_pRobotProtocol) {
        int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
        if (robotResult != 0) {
            LOG_WARNING("Failed to set robot work status to idle, error code: %d\n", robotResult);
        } else {
            LOG_DEBUG("Robot work status set to idle\n");
        }
    }
    
    LOG_INFO("Camera stopped\n");
}

void BeltTearingPresenter::onCameraInitTimer()
{
    // 尝试重新初始化和连接相机
    if (!m_eyeDevice) {
        if (!initializeCamera()) {
            return; // 继续重试
        }
    }

    // 准备相机IP参数（使用配置中的第一个相机）
    const char* pCameraIP = nullptr;
    if (!m_configResult.cameras.empty() && !m_configResult.cameras[0].cameraIP.empty()) {
        pCameraIP = m_configResult.cameras[0].cameraIP.c_str();
    }

    // 尝试连接相机
    int result = m_eyeDevice->OpenDevice(pCameraIP, false, false, true);
    if (result == 0) {
        // 连接成功，停止定时器
        m_cameraInitTimer->stop();
        m_cameraInitialized = true;

        // 开始检测
        result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
        if (result == 0) {
            m_cameraDetecting = true;
            LOG_INFO("Camera connection restored successfully!\n");

            // 设置机械臂工作状态为工作中
            if (m_pRobotProtocol) {
                int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
                if (robotResult != 0) {
                    LOG_WARNING("Failed to set robot work status to working when camera reconnected, error code: %d\n", robotResult);
                } else {
                    LOG_DEBUG("Robot work status set to working when camera reconnected\n");
                }
            }
        } else {
            LOG_ERROR("Failed to start detection after reconnection, error code: %d\n", result);
        }
    } else {
        LOG_WARNING("Still unable to connect to camera, continuing retry...\n");
    }
}

void BeltTearingPresenter::OnStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pInfoParam);
    if (!presenter) return;
    
    LOG_DEBUG("Camera status changed: %d\n", static_cast<int>(eStatus));
    
    // 处理相机状态变化
    switch (eStatus) {
        case keDeviceWorkStatus_Eye_Comming:
            LOG_INFO("Camera connected\n");
            break;
        case keDeviceWorkStatus_Offline:
            LOG_WARNING("Camera disconnected, attempting to reconnect...\n");
            presenter->m_cameraInitialized = false;
            presenter->m_cameraDetecting = false;
            
            // 设置机械臂工作状态为空闲
            if (presenter->m_pRobotProtocol) {
                int robotResult = presenter->m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
                if (robotResult != 0) {
                    LOG_WARNING("Failed to set robot work status to idle when camera disconnected, error code: %d\n", robotResult);
                } else {
                    LOG_DEBUG("Robot work status set to idle when camera disconnected\n");
                }
            }
            
            // 开始重连尝试
            if (!presenter->m_cameraInitTimer->isActive()) {
                presenter->m_cameraInitTimer->start(3000);
            }
            break;
        default:
            break;
    }
}

void BeltTearingPresenter::OnPointCloudCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pParam)
{
    if(keResultDataType_Position != eDataType) return;

    BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pParam);
    if (!presenter || !pLaserLinePoint) return;
    
    // 处理点云数据
    // 将激光线数据添加到队列（用于图像生成和算法检测）
    presenter->addLaserLineToQueue(pLaserLinePoint);
}

void BeltTearingPresenter::sendTearingResults(const std::vector<SSG_beltTearingInfo>& results)
{
    if (results.empty() || m_clients.isEmpty() || !m_tcpServer) {
        return;
    }

    // 将检测结果转换为JSON格式
    QJsonArray tearingArray;

    for (const auto& result : results) {
        QJsonObject tearingObj;
        // tearingObj["level"]         = QString::number(result.level);
        tearingObj["id"]            = QString::number(result.tearID);
        tearingObj["tearStatus"]    = QString::number(static_cast<int>(result.tearStatus));
        tearingObj["tearType"]      = QString::number(result.tearType);
        tearingObj["statLineIdx"]   = QString::number(result.statLineIdx);
        tearingObj["endLineIdx"]    = QString::number(result.endLineIdx);
        tearingObj["tearDepth"]     = QString::number(result.tearDepth);
        tearingObj["tearWidth"]     = QString::number(result.tearWidth);
        double dWidth = result.roi.right - result.roi.left;
        double dLength = result.roi.bottom - result.roi.top;
        tearingObj["tearLength"]    = QString::number(dWidth > dLength ? dWidth : dLength);
        tearingObj["roiLeft"]       = QString::number(result.roi.left);
        tearingObj["roiRight"]      = QString::number(result.roi.right);
        tearingObj["roiTop"]        = QString::number(result.roi.top);
        tearingObj["roiBottom"]     = QString::number(result.roi.bottom);
        // tearingObj["imageFile"]     = QString::fromStdString(result.imageFile);
        // tearingObj["older"]         = QString::fromStdString(result.older);
        tearingObj["timestamp"]     = QDateTime::currentDateTime().toString(Qt::ISODate);

        tearingArray.append(tearingObj);
    }

    // 转换为JSON字符串
    QJsonDocument doc(tearingArray);
    QByteArray message = doc.toJson(QJsonDocument::Compact);

    // 创建数据包
    QByteArray package;
    QDataStream stream(&package, QIODevice::WriteOnly);
    stream.setByteOrder(QDataStream::BigEndian);
    stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(message.size());
    stream.writeRawData(message.constData(), message.size());
    package.append("___END___\r\n");

    // 发送到所有连接的客户端
    bool success = m_tcpServer->SendAllData(package.constData(), package.size());
    if (!success) {
        LOG_WARNING("Failed to send tearing results to all clients\n");
    }
}

bool BeltTearingPresenter::startServer(quint16 port)
{
    if (!m_tcpServer) {
        LOG_ERROR("TCP server not created\n");
        return false;
    }

    // 先停止现有服务器
    stopServer();

    m_port = port;

    // 初始化TCP服务器
    if (!m_tcpServer->Init(port, true)) { // 启用Nagle算法优化
        LOG_ERROR("Failed to initialize TCP server on port %d\n", port);
        return false;
    }

    // 设置事件回调
    m_tcpServer->SetEventCallback(OnServerEvent);

    // 启动TCP服务器
    if (!m_tcpServer->Start(OnServerRecv, false)) { // 不使用自定义协议
        LOG_ERROR("Failed to start TCP server on port %d\n", port);
        return false;
    }

    LOG_INFO("TCP server started on port %d\n", port);
    return true;
}

void BeltTearingPresenter::stopServer()
{
    if (!m_tcpServer) {
        return;
    }

    // 清空客户端映射
    m_clients.clear();

    // 停止TCP服务器
    m_tcpServer->Stop();
    m_tcpServer->Close();

    m_port = 0;
    LOG_INFO("TCP server stopped\n");
}

// 静态回调函数实现
void BeltTearingPresenter::OnServerRecv(const TCPClient* pClient, const char* pData, const unsigned int nLen)
{
    if (s_instance) {
        s_instance->handleServerRecv(pClient, pData, nLen);
    }
}

void BeltTearingPresenter::OnServerEvent(const TCPClient* pClient, TCPServerEventType eventType)
{
    if (s_instance) {
        s_instance->handleServerEvent(pClient, eventType);
    }
}

// 实例方法实现
void BeltTearingPresenter::handleServerRecv(const TCPClient* pClient, const char* pData, const unsigned int nLen)
{
    QString clientId = generateClientId(pClient);
    LOG_DEBUG("Received %d bytes from client %s\n", nLen, clientId.toStdString().c_str());

    // 解析数据包协议头
    if (nLen < 5) {
        LOG_ERROR("Packet too small: %d bytes\n", nLen);
        return;
    }

    quint8 dataType;
    quint32 dataSize;

    QDataStream stream(QByteArray(pData, nLen));
    stream.setByteOrder(QDataStream::BigEndian);
    stream >> dataType >> dataSize;

    // 验证数据包完整性
    if (dataSize + 5 > nLen) {
        LOG_ERROR("Incomplete packet: expected %d+5, got %d bytes\n", dataSize, nLen);
        return;
    }

    QByteArray payloadData(pData + 5, dataSize);

    // LOG_DEBUG("Parsed packet: dataType=%d, dataSize=%d, payload_size=%d\n", dataType, dataSize, payloadData.size());

    switch (static_cast<ByteDataType>(dataType)) {
    case ByteDataType::Text:
        // 处理文本数据
        handleAlgorithmParameterUpdate(payloadData);
        break;
        
    case ByteDataType::ReadConfig:
        // 处理读取配置请求
        handleReadConfig(pClient);
        break;
        
    case ByteDataType::WriteConfig:
        // 处理写入配置请求
        handleWriteConfig(payloadData);
        break;

    default:
        LOG_ERROR("Unknown data type %d\n", dataType);
        break;
    }
}

void BeltTearingPresenter::handleServerEvent(const TCPClient* pClient, TCPServerEventType eventType)
{
    QString clientId = generateClientId(pClient);

    switch (eventType) {
    case TCP_EVENT_CLIENT_CONNECTED:
        m_clients[clientId] = pClient;
        LOG_INFO("Client connected: %s\n", clientId.toStdString().c_str());
        break;

    case TCP_EVENT_CLIENT_DISCONNECTED:
        m_clients.remove(clientId);
        LOG_INFO("Client disconnected: %s\n", clientId.toStdString().c_str());
        break;

    case TCP_EVENT_CLIENT_EXCEPTION:
        m_clients.remove(clientId);
        LOG_WARNING("Client exception: %s\n", clientId.toStdString().c_str());
        break;
    }
}

QString BeltTearingPresenter::generateClientId(const TCPClient* client)
{
    return QString("Client_%1").arg(client->m_nFD);
}

// 激光线队列管理方法实现
void BeltTearingPresenter::addLaserLineToQueue(const SVzLaserLineData* laserLine)
{
    if(laserLine == nullptr || laserLine->nPointCount <= 0){
        return;
    }

    // 创建激光线数据的深拷贝
    SVzLaserLineData copiedLine = *laserLine;

    // 深拷贝点云数据
    size_t pointDataSize = laserLine->nPointCount * sizeof(SVzNL3DPosition);
    copiedLine.p3DPoint = static_cast<SVzNL3DPosition*>(malloc(pointDataSize));
    if (copiedLine.p3DPoint) {
        memcpy(copiedLine.p3DPoint, laserLine->p3DPoint, pointDataSize);
    }

    // 添加数据到队列的锁作用域
    {
        std::lock_guard<std::mutex> lock(m_queueMutex);
        // 添加到队列
        m_laserLineQueue.push_back(copiedLine);
        m_lineCounter++;

        // 管理队列大小
        while (m_laserLineQueue.size() > MAX_QUEUE_SIZE) {
            SVzLaserLineData oldLine = m_laserLineQueue.front();
            m_laserLineQueue.pop_front();

            // 释放深拷贝的点云数据内存
            if (oldLine.p3DPoint) {
                free(oldLine.p3DPoint);
                oldLine.p3DPoint = nullptr;
            }
        }
    }

    // 每到图像生成间隔，提交图像生成任务给工作线程
    if (m_lineCounter % GENERATION_INTERVAL == 0) {
        m_algoDetectCondition.notify_one();
    }
}

void BeltTearingPresenter::sendImageToClients(const QImage& image)
{
    if (image.isNull() || m_clients.isEmpty() || !m_tcpServer) {
        return;
    }

    try {
        // 将图像转换为字节数组
        QByteArray imageData;
        QBuffer buffer(&imageData);
        buffer.open(QIODevice::WriteOnly);

        // 保存为JPEG格式以减少数据大小
        if (!image.save(&buffer, "JPEG", 50)) {
            LOG_ERROR("Failed to convert image to JPEG format\n");
            return;
        }

        // 构造数据包
        QByteArray packet;
        QDataStream stream(&packet, QIODevice::WriteOnly);
        stream.setByteOrder(QDataStream::BigEndian);

        // 写入数据类型标识（图像类型）
        stream << static_cast<quint8>(ByteDataType::Image);

        // 写入图像数据大小
        stream << static_cast<quint32>(imageData.size());

        // 写入图像数据
        stream.writeRawData(imageData.constData(), imageData.size());
        packet.append("___END___\r\n");


        // 发送给所有连接的客户端
        bool success = m_tcpServer->SendAllData(packet.constData(), packet.size());
        if (!success) {
            LOG_WARNING("Failed to send image data to all clients\n");
        }
        // LOG_DEBUG("Sent image data to %d clients datalen %d\n", m_clients.size(), imageData.size());

    } catch (const std::exception& e) {
        LOG_ERROR("Error sending image to clients: %s\n", e.what());
    }
}

SSG_beltTearingParam BeltTearingPresenter::configToSDKParam(const BeltTearingConfigResult& config) const
{
    SSG_beltTearingParam sdkParam;

    // 基本参数
    sdkParam.scanXScale = config.algorithmParams.beltTearingParam.scanXScale;
    sdkParam.scanYScale = config.algorithmParams.beltTearingParam.scanYScale;
    sdkParam.differnceBinTh = config.algorithmParams.beltTearingParam.differnceBinTh;
    sdkParam.tearingMinLen = config.algorithmParams.beltTearingParam.tearingMinLen;
    sdkParam.tearingMinGap = config.algorithmParams.beltTearingParam.tearingMinGap;

    // 特征提取参数
    sdkParam.extractPara.sameGapTh = config.algorithmParams.beltTearingParam.sameGapTh;
    sdkParam.extractPara.gapChkWin = config.algorithmParams.beltTearingParam.gapChkWin;

    return sdkParam;
}

BeltTearingConfigResult BeltTearingPresenter::sdkToConfigParam(const SSG_beltTearingParam& sdkParam) const
{
    BeltTearingConfigResult config;

    // 基本参数
    config.algorithmParams.beltTearingParam.scanXScale = sdkParam.scanXScale;
    config.algorithmParams.beltTearingParam.scanYScale = sdkParam.scanYScale;
    config.algorithmParams.beltTearingParam.differnceBinTh = sdkParam.differnceBinTh;
    config.algorithmParams.beltTearingParam.tearingMinLen = sdkParam.tearingMinLen;
    config.algorithmParams.beltTearingParam.tearingMinGap = sdkParam.tearingMinGap;

    // 特征提取参数
    config.algorithmParams.beltTearingParam.sameGapTh = sdkParam.extractPara.sameGapTh;
    config.algorithmParams.beltTearingParam.gapChkWin = sdkParam.extractPara.gapChkWin;

    return config;
}

void BeltTearingPresenter::handleAlgorithmParameterUpdate(const QByteArray& paramData)
{
    try {
        // 解析JSON数据
        QJsonDocument doc = QJsonDocument::fromJson(paramData);
        if (!doc.isObject()) {
            LOG_WARNING("Invalid JSON format in parameter update\n");
            return;
        }

        QJsonObject paramObj = doc.object();
        QString command = paramObj["command"].toString();

        if (command == "setAlgorithmParams") {
            // 处理算法参数设置
            handleSetAlgorithmParams(paramObj);
        } else if (command == "getServerInfo") {
            // 处理服务器信息获取请求
            handleGetServerInfo(paramObj);
        } else if (command == "resetDetect") {
            // 处理重新检测请求
            LOG_INFO("Received reset detect command from client\n");
            ResetDetect();
            
            // 发送响应给客户端
            QJsonObject responseObj;
            responseObj["command"] = "resetDetectResponse";
            responseObj["status"] = "success";
            responseObj["message"] = "Detection reset completed";
            responseObj["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate);
            
            QJsonDocument responseDoc(responseObj);
            QByteArray responseData = responseDoc.toJson();
            
            // 构建数据包并发送给所有客户端
            if (!m_clients.isEmpty() && m_tcpServer) {
                QByteArray package;
                QDataStream stream(&package, QIODevice::WriteOnly);
                stream.setByteOrder(QDataStream::BigEndian);
                stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(responseData.size());
                stream.writeRawData(responseData.constData(), responseData.size());
                package.append("___END___\r\n");

                bool success = m_tcpServer->SendAllData(package.constData(), package.size());
                if (success) {
                    LOG_INFO("Reset detect response sent to clients\n");
                } else {
                    LOG_WARNING("Failed to send reset detect response to clients\n");
                }
            }
        } else {
            LOG_WARNING("Unknown command: %s\n", command.toStdString().c_str());
            return;
        }

    } catch (const std::exception& e) {
        LOG_ERROR("Error processing algorithm parameter update: %s\n", e.what());
    }
}

void BeltTearingPresenter::handleSetAlgorithmParams(const QJsonObject& paramObj)
{
    // 提取算法参数
    double scanXScale = paramObj["scanXScale"].toDouble();
    double scanYScale = paramObj["scanYScale"].toDouble();
    double differnceBinTh = paramObj["differnceBinTh"].toDouble();
    double tearingMinLen = paramObj["tearingMinLen"].toDouble();
    double tearingMinGap = paramObj["tearingMinGap"].toDouble();

    // 更新特征提取参数
    m_algorithmParam.extractPara.sameGapTh = paramObj["sameGapTh"].toDouble();
    m_algorithmParam.extractPara.gapChkWin = paramObj["gapChkWin"].toInt();

    // 更新SDK算法参数
    m_algorithmParam.scanXScale = scanXScale;
    m_algorithmParam.scanYScale = scanYScale;
    m_algorithmParam.differnceBinTh = differnceBinTh;
    m_algorithmParam.tearingMinLen = tearingMinLen;
    m_algorithmParam.tearingMinGap = tearingMinGap;

    LOG_INFO("Algorithm parameters updated: scanXScale=%f, scanYScale=%f, differnceBinTh=%f, tearingMinLen=%f, tearingMinGap=%f\n",
             scanXScale, scanYScale, differnceBinTh, tearingMinLen, tearingMinGap);


    // 保存参数到配置文件
    if (m_config) {
        BeltTearingConfigResult currentConfig = sdkToConfigParam(m_algorithmParam);
        QString configPath = PathManager::GetConfigFilePath();

        if (m_config->SaveConfig(configPath.toStdString(), currentConfig)) {
            LOG_INFO("Algorithm parameters saved to config file: %s\n", configPath.toStdString().c_str());
        } else {
            LOG_ERROR("Failed to save algorithm parameters to config file\n");
        }
    }
}

void BeltTearingPresenter::handleGetServerInfo(const QJsonObject& requestObj)
{
    // 创建服务器信息响应
    QJsonObject responseObj;
    responseObj["command"] = "serverInfoResponse";
    responseObj["requestId"] = requestObj["requestId"].toString();

    // 服务器基本信息
    QJsonObject serverInfo;
    serverInfo["name"]      = BELT_TEARING_SERVER_PRODUCT_NAME;
    serverInfo["version"]   = BELT_TEARING_SERVER_VERSION_STRING;
    serverInfo["buildInfo"] = BELT_TEARING_SERVER_PLATFORM;
    serverInfo["status"] = "running";
    serverInfo["port"] = static_cast<int>(getServerPort());

    // 当前算法参数
    QJsonObject algorithmParams;
    algorithmParams["scanXScale"] = m_algorithmParam.scanXScale;
    algorithmParams["scanYScale"] = m_algorithmParam.scanYScale;
    algorithmParams["differnceBinTh"] = m_algorithmParam.differnceBinTh;
    algorithmParams["tearingMinLen"] = m_algorithmParam.tearingMinLen;
    algorithmParams["tearingMinGap"] = m_algorithmParam.tearingMinGap;

    responseObj["serverInfo"] = serverInfo;
    responseObj["algorithmParams"] = algorithmParams;
    responseObj["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate);

    // 将响应发送给所有客户端
    QJsonDocument responseDoc(responseObj);
    QByteArray responseData = responseDoc.toJson();

    // 使用现有的sendTearingResults传输机制发送响应
    if (!m_clients.isEmpty() && m_tcpServer) {
        // 创建数据包
        QByteArray package;
        QDataStream stream(&package, QIODevice::WriteOnly);
        stream.setByteOrder(QDataStream::BigEndian);
        stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(responseData.size());
        stream.writeRawData(responseData.constData(), responseData.size());
        package.append("___END___\r\n");

        // 发送到所有连接的客户端
        bool success = m_tcpServer->SendAllData(package.constData(), package.size());
        if (success) {
            LOG_INFO("Server info response sent to %d clients\n", m_clients.size());
        } else {
            LOG_WARNING("Failed to send server info response to clients\n");
        }
    }
}

void BeltTearingPresenter::handleReadConfig(const TCPClient* pClient)
{
    try {
        // 直接使用当前内存中的配置参数
        BeltTearingConfigResult configResult = sdkToConfigParam(m_algorithmParam);
        
        // 将配置转换为JSON格式
        QJsonObject configObj;
        
        // 服务器配置
        QJsonArray serversArray;
        for (const auto& server : m_configResult.servers) {
            QJsonObject serverObj;
            serverObj["name"] = QString::fromStdString(server.name);
            serverObj["ip"] = QString::fromStdString(server.ip);
            serverObj["port"] = server.port;
            serversArray.append(serverObj);
        }
        configObj["servers"] = serversArray;
        
        // 算法参数
        QJsonObject algorithmParams;
        algorithmParams["scanXScale"] = configResult.algorithmParams.beltTearingParam.scanXScale;
        algorithmParams["scanYScale"] = configResult.algorithmParams.beltTearingParam.scanYScale;
        algorithmParams["differnceBinTh"] = configResult.algorithmParams.beltTearingParam.differnceBinTh;
        algorithmParams["tearingMinLen"] = configResult.algorithmParams.beltTearingParam.tearingMinLen;
        algorithmParams["tearingMinGap"] = configResult.algorithmParams.beltTearingParam.tearingMinGap;
        algorithmParams["sameGapTh"] = configResult.algorithmParams.beltTearingParam.sameGapTh;
        algorithmParams["gapChkWin"] = configResult.algorithmParams.beltTearingParam.gapChkWin;
        configObj["algorithmParams"] = algorithmParams;
        
        // 调试参数
        QJsonObject debugParams;
        debugParams["enableDebug"] = m_configResult.debugParam.enableDebug;
        debugParams["saveDebugImage"] = m_configResult.debugParam.saveDebugImage;
        debugParams["printDetailLog"] = m_configResult.debugParam.printDetailLog;
        debugParams["debugOutputPath"] = QString::fromStdString(m_configResult.debugParam.debugOutputPath);
        configObj["debugParams"] = debugParams;
        
        // 项目类型
        configObj["projectType"] = static_cast<int>(m_configResult.projectType);
        
        // 服务端端口
        configObj["serverPort"] = m_configResult.serverPort;
        
        // 创建响应对象
        QJsonObject responseObj;
        responseObj["command"] = "configResponse";
        responseObj["config"] = configObj;
        
        // 转换为JSON数据
        QJsonDocument responseDoc(responseObj);
        QByteArray responseData = responseDoc.toJson();
        
        // 构建数据包
        QByteArray packet;
        QDataStream stream(&packet, QIODevice::WriteOnly);
        stream.setByteOrder(QDataStream::BigEndian);
        stream << static_cast<quint8>(ByteDataType::ReadConfig) << static_cast<quint32>(responseData.size());
        stream.writeRawData(responseData.constData(), responseData.size());
        packet.append("___END___\r\n");
        
        // 发送响应
        if (m_tcpServer && pClient) {
            bool success = m_tcpServer->SendData(pClient, packet.constData(), packet.size());
            if (success) {
                LOG_INFO("Configuration sent to client successfully\n");
            } else {
                LOG_ERROR("Failed to send configuration to client\n");
            }
        }
    } catch (const std::exception& e) {
        LOG_ERROR("Error handling ReadConfig: %s\n", e.what());
    }
}

void BeltTearingPresenter::handleWriteConfig(const QByteArray& paramData)
{
    try {
        // 解析JSON数据
        QJsonDocument doc = QJsonDocument::fromJson(paramData);
        if (!doc.isObject()) {
            LOG_WARNING("Invalid JSON format in WriteConfig\n");
            return;
        }
        
        QJsonObject paramObj = doc.object();
        
        // 检查是否是来自 dialogalgoarg 的简单参数格式
        if (paramObj.contains("command") && paramObj["command"].toString() == "setAlgorithmParams") {
            // 处理来自 dialogalgoarg 的算法参数更新
            handleSetAlgorithmParams(paramObj);
        }


    } catch (const std::exception& e) {
        LOG_ERROR("Error handling WriteConfig: %s\n", e.what());
    }
}

void BeltTearingPresenter::ResetDetect()
{
    LOG_INFO("Resetting detection system\n");

    stopCamera();

    m_hLineWorkers.clear();
    m_beltTearings_new.clear();
    m_beltTearings_growing.clear();
    m_beltTearings_ended.clear();
    m_beltTearings_unknown.clear();

    m_bInitAlgo = false;

    std::lock_guard<std::mutex> lock(m_queueMutex);
    for (auto& line : m_laserLineQueue) {
        if (line.p3DPoint) {
            free(line.p3DPoint);
            line.p3DPoint = nullptr;
        }
    }
    m_laserLineQueue.clear();

    // 清空Modbus检测结果数据
    if (m_pRobotProtocol) {
        int ret = m_pRobotProtocol->ClearDetectionData();
        if (ret != 0) {
            LOG_WARNING("Failed to clear Modbus detection data during reset, error code: %d\n", ret);
        } else {
            LOG_INFO("Modbus detection data cleared during reset\n");
        }
    }

    startCamera();

    LOG_INFO("Detection system reset completed\n");
}

void BeltTearingPresenter::StopWork()
{
    LOG_INFO("Stopping work - camera and detection\n");

    // 停止相机
    stopCamera();

    // 设置机械臂工作状态为空闲
    if (m_pRobotProtocol) {
        m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
    }

    LOG_INFO("Work stopped successfully\n");
}

void BeltTearingPresenter::StartWork()
{
    LOG_INFO("Starting work - camera and detection\n");

    // 启动相机
    startCamera();

    // 设置机械臂工作状态为工作中（在startCamera中已经设置）

    LOG_INFO("Work started successfully\n");
}

void BeltTearingPresenter::_AlgoDetectThread()
{
    while (m_bAlgoDetectThreadRunning) {
        // 等待条件变量通知
        std::unique_lock<std::mutex> lock(m_algoDetectMutex);
        m_algoDetectCondition.wait(lock);
        
        // 如果线程停止运行，退出循环
        if (!m_bAlgoDetectThreadRunning) {
            break;
        }
        
        // 执行检测任务
        _DetectTask();
    }
}

int BeltTearingPresenter::_DetectTask()
{
    std::lock_guard<std::mutex> lock(m_detectionDataMutex);

    // 临时存储从队列中取出的数据，避免重复处理    
    std::vector<SVzNL3DLaserLine>               algorithmDataCache;
    std::vector<std::vector<SVzNL3DPosition>>   imageScanLines;

    // 从队列中获取数据
    {
        std::lock_guard<std::mutex> queueLock(m_queueMutex);
        if (m_laserLineQueue.empty()) {
            return 0;  // 没有数据可处理
        }
        
        // 只保留最后的GENERATION_INTERVAL大小的数据进行检测
        algorithmDataCache.reserve(GENERATION_INTERVAL);
        int cacheSize = static_cast<int>(m_laserLineQueue.size()) - GENERATION_INTERVAL;
        int startIndex = 0 > cacheSize ? 0 : cacheSize;
        for (auto it = m_laserLineQueue.begin() + startIndex; it != m_laserLineQueue.end(); ++it) {
            const auto& laserLine = *it;
            SVzNL3DLaserLine algorithmData;
            algorithmData.nTimeStamp = laserLine.llTimeStamp;
            algorithmData.nPositionCnt = laserLine.nPointCount;
            
            // 深拷贝点云数据
            if (laserLine.nPointCount > 0 && laserLine.p3DPoint) {
                size_t pointDataSize = laserLine.nPointCount * sizeof(SVzNL3DPosition);
                algorithmData.p3DPosition = static_cast<SVzNL3DPosition*>(malloc(pointDataSize));
                if (algorithmData.p3DPosition) {
                    memcpy(algorithmData.p3DPosition, laserLine.p3DPoint, pointDataSize);
                }
            } else {
                algorithmData.p3DPosition = nullptr;
            }
            
            algorithmDataCache.push_back(algorithmData);
        }
        
        // 准备扫描线数据用于图像生成
        imageScanLines.reserve(m_laserLineQueue.size());
        for (const auto& line : m_laserLineQueue) {
            // 增加安全检查，确保指针有效
            if (line.p3DPoint && line.nPointCount > 0) {
                std::vector<SVzNL3DPosition> linePoints;
                linePoints.reserve(line.nPointCount); // 预分配内存提高效率

                SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(line.p3DPoint);

                // 使用更高效的批量复制
                linePoints.assign(p3DPoints, p3DPoints + line.nPointCount);
                imageScanLines.emplace_back(std::move(linePoints)); // 使用move避免拷贝
            }
        }
    }
    
    // 调用SDK算法进行皮带撕裂检测
    int errorCode = 0;
    std::vector<SSG_beltTearingInfo> allTearings;
    
    // 初始化算法（如果尚未初始化）
    if (!m_bInitAlgo) {
        const SVzNL3DLaserLine& firstLine = algorithmDataCache.front();
        m_hLineWorkers.resize(firstLine.nPositionCnt);

        m_beltTearings_new.clear();
        m_beltTearings_new.shrink_to_fit();
        m_beltTearings_growing.clear();
        m_beltTearings_growing.shrink_to_fit();
        m_beltTearings_ended.clear();
        m_beltTearings_ended.shrink_to_fit();
        m_beltTearings_unknown.clear();
        m_beltTearings_unknown.shrink_to_fit();
        
        // 复位内部静态变量
        sg_detectBeltTearing(nullptr, 0, 0, &errorCode,
            m_hLineWorkers,
            m_beltTearings_new, m_beltTearings_growing, m_beltTearings_ended, m_beltTearings_unknown,
            m_algorithmParam
        );
        m_bInitAlgo = true;
    }
    
    // 对缓存中的每一帧数据进行检测
    for (auto& algorithmData : algorithmDataCache) {
        // 直接调用皮带撕裂检测算法
        sg_detectBeltTearing(&algorithmData, static_cast<int>(algorithmData.nTimeStamp), 
            algorithmData.nPositionCnt, &errorCode,
            m_hLineWorkers,
            m_beltTearings_new, m_beltTearings_growing, m_beltTearings_ended, m_beltTearings_unknown,
            m_algorithmParam
        );
                
        // 合并各个撕裂结果容器
#if 1
        _MergeAndReplace(allTearings, m_beltTearings_new);
        _MergeAndReplace(allTearings, m_beltTearings_growing);
        _MergeAndReplace(allTearings, m_beltTearings_ended);
        // _MergeAndReplace(allTearings, m_beltTearings_unknown);
#else
        allTearings.insert(allTearings.end(), m_beltTearings_new.begin(), m_beltTearings_new.end());
        allTearings.insert(allTearings.end(), m_beltTearings_growing.begin(), m_beltTearings_growing.end());
        allTearings.insert(allTearings.end(), m_beltTearings_ended.begin(), m_beltTearings_ended.end());
        // allTearings.insert(allTearings.end(), m_beltTearings_unknown.begin(), m_beltTearings_unknown.end());
#endif
    }
    
    // 发送检测结果
    if (!allTearings.empty()) {
        sendTearingResults(allTearings);
        SendDetectionResultToRobot(allTearings);  // 发送检测结果到机械臂
    }

    // 释放深拷贝的点云数据
    for (auto& algorithmData : algorithmDataCache) {
        if (algorithmData.p3DPosition) {
            free(algorithmData.p3DPosition);
            algorithmData.p3DPosition = nullptr;
        }
    }
    
    // 在算法处理完成后，生成带撕裂检测结果的图像
    QImage image = PointCloudImageUtils::GeneratePointCloudImage(imageScanLines, allTearings, 800, 600);
    sendImageToClients(image);
    
    return 0;  // 成功完成检测任务
}

void BeltTearingPresenter::_MergeAndReplace(std::vector<SSG_beltTearingInfo>& allTearings, const std::vector<SSG_beltTearingInfo>& source)
{
    for (const auto& newItem : source) 
    {
        bool found = false;
        // 检查是否已存在相同ID的项
        for (auto& existingItem : allTearings) {
            if (existingItem.tearID == newItem.tearID) {
                // 如果存在，用新数据替换旧数据
                existingItem = newItem;
                found = true;
                break;
            }
        }
        // 如果不存在相同ID的项，则添加新项
        if (!found) {
            allTearings.push_back(newItem);
        }
    }
}



// 初始化Modbus-RTU主端
int BeltTearingPresenter::InitModbusRTUMaster()
{
    // 创建ModbusRTUMaster实例
    m_pModbusRTUMaster = new ModbusRTUMaster();
    
    // 初始化Modbus-RTU主端，打开/dev/ttyS4设备，波特率115200，无校验，8数据位，1停止位
    int nRet = m_pModbusRTUMaster->Initialize("/dev/ttyS4", 115200, 'N', 8, 1);
    if (nRet != 0) {
        LOG_ERROR("Failed to initialize Modbus RTU master, error code: %d\n", nRet);
        return nRet;
    }
    
    // 设置温度回调函数
    m_pModbusRTUMaster->SetTemperatureCallback([this](float temperature) {
        this->SendTemperatureData(temperature);
    });
    
    // 启动定时器读取温度数据
    m_pModbusRTUMaster->StartReading();
    
    m_bModbusRTUConnected = true;
    LOG_INFO("Modbus RTU master initialized successfully\n");
    return 0;
}


// 发送温度数据给所有客户端
void BeltTearingPresenter::SendTemperatureData(float temperature)
{
    // 检查TCP服务器是否已初始化
    if (!m_tcpServer) {
        LOG_WARNING("TCP server not initialized, cannot send temperature data\n");
        return;
    }

    // 将温度数据转换为JSON格式
    QJsonObject temperatureObj;
    temperatureObj["id"] = QString::number(0);  // 温度目标ID为0
    temperatureObj["status"] = QString::number(1);  // 状态1表示有效数据
    temperatureObj["value"] = QString::number(temperature);  // 宽度字段存储温度值
    temperatureObj["type"] = QString("temperature");  // 数据类型标识
    
    QJsonArray temperatureArray;
    temperatureArray.append(temperatureObj);
    
    // 转换为JSON字符串
    QJsonDocument doc(temperatureArray);
    QByteArray message = doc.toJson(QJsonDocument::Compact);

    // 创建数据包
    QByteArray package;
    QDataStream stream(&package, QIODevice::WriteOnly);
    stream.setByteOrder(QDataStream::BigEndian);
    stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(message.size());
    stream.writeRawData(message.constData(), message.size());
    package.append("___END___\r\n");

    // 通过TCP服务器发送到所有连接的客户端
    bool success = m_tcpServer->SendAllData(package.constData(), package.size());
    if (!success) {
        LOG_WARNING("Failed to send temperature data to all clients\n");
    } else {
        LOG_DEBUG("Temperature data sent successfully: %.2f°C\n", temperature);
    }
}


// 初始化机械臂协议
int BeltTearingPresenter::InitRobotProtocol()
{
    LOG_DEBUG("Start initializing robot protocol\n");
    
    // 创建RobotProtocol实例
    if(nullptr == m_pRobotProtocol){
        m_pRobotProtocol = new RobotProtocol();
    }

    // 初始化协议服务（使用端口502）
    int nRet = m_pRobotProtocol->Initialize(502);

    // 设置连接状态回调
    m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
        this->OnRobotConnectionChanged(connected);
    });

    // 设置工作信号回调
    m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
        return this->OnRobotWorkSignal(startWork, cameraIndex);
    });

    // 设置系统控制回调
    m_pRobotProtocol->SetSystemControlCallback([this](uint16_t command) {
        switch(command) {
            case 0:  // 停止工作
                LOG_INFO("ModbusTCP command: Stop work\n");
                this->StopWork();
                break;
            case 1:  // 开始工作
                LOG_INFO("ModbusTCP command: Start work\n");
                this->StartWork();
                break;
            case 2:  // 复位检测
                LOG_INFO("ModbusTCP command: Reset detect\n");
                this->ResetDetect();
                break;
            default:
                LOG_WARNING("Unknown ModbusTCP command: %d\n", command);
                break;
        }
    });

    LOG_INFO("Robot protocol initialization completed successfully\n");
    return nRet;
}

// 机械臂连接状态改变回调
void BeltTearingPresenter::OnRobotConnectionChanged(bool connected)
{
    LOG_INFO("Robot connection status changed: %s\n", connected ? "Connected" : "Disconnected");
    m_bRobotConnected = connected;
    
    // 可以在这里添加其他连接状态改变时需要处理的逻辑
    if (connected) {
        LOG_INFO("Robot connected successfully\n");
    } else {
        LOG_WARNING("Robot disconnected\n");
    }
}


// 机械臂工作信号回调
bool BeltTearingPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
{
    return true;  // 返回处理结果
}

// 发送检测结果到机械臂
void BeltTearingPresenter::SendDetectionResultToRobot(const std::vector<SSG_beltTearingInfo>& detectionResults)
{
    // 检查RobotProtocol是否已初始化且连接正常
    if (!m_pRobotProtocol || !m_bRobotConnected) {
        LOG_WARNING("Robot protocol not initialized or not connected, cannot send detection results\n");
        return;
    }

    // 准备发送给机械臂的数据结构
    MultiTargetData multiTargetData;

    // 将皮带撕裂检测结果转换为机械臂可识别的数据
    for (const auto& result : detectionResults) {
        // 创建目标结果
        TargetResult target;
        target.id = result.tearID;
        target.status = static_cast<uint16_t>(result.tearStatus);
        target.width = static_cast<float>(result.tearWidth);
        target.depth = static_cast<float>(result.tearDepth);
        
        // 添加到目标列表
        multiTargetData.targets.push_back(target);
    }

    // 调用RobotProtocol接口发送数据
    m_pRobotProtocol->SetMultiTargetData(multiTargetData);
}