GrabBag/App/BeltTearing/BeltTearingServer/BeltTearingPresenter.cpp

#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 <QFont>
#include <cmath>

#include "IVrUtils.h"
#include "VrError.h"
#include <algorithm>

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

BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
    : QObject(parent)
    , m_tcpServer(nullptr)
    , m_tearingProtocol(nullptr)
    , m_config(nullptr)
    , m_eyeDevice(nullptr)
    , m_cameraInitTimer(new QTimer(this))
    , m_cameraInitialized(false)
    , m_cameraDetecting(false)
    , m_lineCounter(0)
    , m_maxQueueSize(300)              // 默认值
    , m_generationInterval(100)        // 默认值
    , m_bAlgoDetectThreadRunning(false)
    , m_pRobotProtocol(nullptr)   // 初始化RobotProtocol指针
    , m_pModbusRTUMaster(nullptr) // 初始化ModbusRTUMaster指针
    , m_bRobotConnected(false)   // 初始化连接状态
    , m_simulationTearIdCounter(1000)  // 仿真撕裂ID计数器初始值
    , m_simulationCallCount(0)         // 仿真调用次数初始值
{
    // 设置静态实例
    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;
    }


    InitModbusRTUMaster();
}

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

    // 释放TearingTcpProtocol
    if (m_tearingProtocol) {
        delete m_tearingProtocol;
        m_tearingProtocol = nullptr;
    }

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

    // 释放RobotProtocolSimplified资源
    if (m_pRobotProtocolSimplified) {
        m_pRobotProtocolSimplified->Deinitialize();
        delete m_pRobotProtocolSimplified;
        m_pRobotProtocolSimplified = 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());

        // 应用队列处理参数（如果配置文件中没有，将使用构造函数中的默认值300和100）
        if (m_configResult.queueProcessParam.maxQueueSize > 0) {
            m_maxQueueSize = m_configResult.queueProcessParam.maxQueueSize;
        }
        if (m_configResult.queueProcessParam.generationInterval > 0) {
            m_generationInterval = m_configResult.queueProcessParam.generationInterval;
        }
        LOG_INFO("  Max Queue Size: %d\n", m_maxQueueSize);
        LOG_INFO("  Generation Interval: %d\n", m_generationInterval);

        // 应用算法参数
        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");

    if (configResult.serverPort != m_configResult.serverPort)
    {
        // 如果服务器端口改变，可能需要重启服务器
        LOG_INFO("Server port changed, restarting server...\n");
        stopServer();
        startServer(configResult.serverPort);
    }
    
    // 更新配置结果
    m_configResult = configResult;

    // 重新应用算法参数
    applyAlgorithmParameters(configResult.algorithmParams);
}

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;

    // 使用成员变量（开流时会清除）
    m_simulationCallCount++;

    m_simulationTearIdCounter = 2629344;

    // 固定生成2个撕裂，长度逐渐增大
    for (int i = 0; i < 2; i++) {
        SSG_beltTearingInfo tear;
        tear.tearID = m_simulationTearIdCounter + i;
        tear.tearStatus = keSG_tearStatus_Growing;

        // 撕裂长度：基础长度 + 调用次数 * 10
        float tearLength = 30.0f + m_simulationCallCount * 10.0f + i * 5.0f;

        // 设置ROI
        tear.roi.left = 10.0f + i * 50.0f;
        tear.roi.top = 0.0f;
        tear.roi.right = tear.roi.left + tearLength;
        tear.roi.bottom = 10.0f;

        // 设置撕裂宽度和深度
        tear.tearWidth = 15.0f + i * 5.0f;
        tear.tearDepth = 8.0f + i * 2.0f;

        simulationResults.push_back(tear);
    }

    // 发送撕裂结果到所有客户端
    sendTearingResults(simulationResults);
    SendDetectionResultToRobot(simulationResults);  // 发送检测结果到机械臂

    m_tearingProtocol->sendDetectResult(simulationResults, QImage());
}

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

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

    // 准备相机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 result;  // 返回错误码
    }

    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 result;  // 返回错误码
    }

    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);
            return robotResult;  // 返回错误码
        } else {
            LOG_DEBUG("Robot work status set to working\n");
        }
    }
    
    return SUCCESS;  // 成功
}

int 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);
            return ERR_CODE(DEV_CTRL_ERR);  // 返回设备控制错误码
        } else {
            LOG_DEBUG("Robot work status set to idle\n");
        }
    }
    
    LOG_INFO("Camera stopped\n");
    return SUCCESS;  // 成功
}

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;
    m_tcpPort = m_configResult.tcpPort;  // 从配置读取新协议端口

    // 初始化旧协议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 (old protocol) started on port %d\n", port);

    // 创建并启动TearingTcpProtocol
    if (!m_tearingProtocol) {
        m_tearingProtocol = new TearingTcpProtocol(this);

        // 设置TCP端口
        m_tearingProtocol->setTcpPort(m_tcpPort);

        // 设置速度控制回调
        m_tearingProtocol->setSpeedCallback([this](int speed) -> int {
            LOG_INFO("Speed callback triggered: %d mm/s\n", speed);

            // 调用相机接口设置速度
            if (m_eyeDevice) {
                // 将速度从 mm/s 转换为相机摆动速度参数（根据实际需要调整转换公式）
                // 这里假设直接使用速度值，实际可能需要根据相机文档进行单位转换
                float fSpeed = static_cast<float>(speed);

                int result = m_eyeDevice->SetSwingSpeed(fSpeed);
                if (result == 0) {
                    LOG_INFO("Camera swing speed set successfully: %.2f\n", fSpeed);
                    return SUCCESS;  // 成功
                } else {
                    LOG_ERROR("Failed to set camera swing speed, error code: %d\n", result);
                    return result;  // 返回错误码
                }
            } else {
                LOG_WARNING("Camera device not initialized, cannot set speed\n");
                return ERR_CODE(DEV_NO_OPEN);  // 设备未初始化错误
            }
        });

        // 设置启停控制回调
        m_tearingProtocol->setControlCallback([this](bool control) -> int {
            LOG_INFO("Control callback triggered: %s\n", control ? "start" : "stop");
            int result = 0;
            if (control) {
                result = StartWork();
            } else {
                result = StopWork();
            }
            return result;
        });

        // 启动协议处理
        m_tearingProtocol->start(30); // 30秒心跳间隔
    }

    LOG_INFO("TearingTcpProtocol started on port %d\n", m_tcpPort);

    return true;
}

void BeltTearingPresenter::stopServer()
{
    // 停止TearingTcpProtocol
    if (m_tearingProtocol) {
        m_tearingProtocol->stop();
    }

    // 停止旧协议TCP服务器
    if (m_tcpServer) {
        // 清空客户端映射
        m_clients.clear();

        m_tcpServer->Stop();
        m_tcpServer->Close();
        LOG_INFO("TCP server stopped\n");
    }

    m_port = 0;
    m_tcpPort = 0;
}

// 静态回调函数实现
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() > static_cast<size_t>(m_maxQueueSize)) {
            SVzLaserLineData oldLine = m_laserLineQueue.front();
            m_laserLineQueue.pop_front();

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

    // 每到图像生成间隔，提交图像生成任务给工作线程
    if (m_lineCounter % m_generationInterval == 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 (paramObj.contains("maxQueueSize")) {
        int newMaxQueueSize = paramObj["maxQueueSize"].toInt();
        if (newMaxQueueSize > 0) {
            m_maxQueueSize = newMaxQueueSize;
            LOG_INFO("Max queue size updated to: %d\n", m_maxQueueSize);
        }
    }

    if (paramObj.contains("generationInterval")) {
        int newGenerationInterval = paramObj["generationInterval"].toInt();
        if (newGenerationInterval > 0) {
            m_generationInterval = newGenerationInterval;
            LOG_INFO("Generation interval updated to: %d\n", m_generationInterval);
        }
    }

    // 处理相机IP配置
    if (paramObj.contains("cameraIP")) {
        QString newCameraIP = paramObj["cameraIP"].toString();

        // 更新配置中的相机IP
        if (!m_configResult.cameras.empty()) {
            std::string oldIP = m_configResult.cameras[0].cameraIP;
            m_configResult.cameras[0].cameraIP = newCameraIP.toStdString();

            if (oldIP != m_configResult.cameras[0].cameraIP) {
                LOG_INFO("Camera IP updated from [%s] to [%s]\n", oldIP.c_str(), m_configResult.cameras[0].cameraIP.c_str());
                LOG_INFO("Camera IP change requires restart to take effect\n");
            }
        } else {
            // 如果相机列表为空，创建新的相机配置
            CameraParam newCamera;
            newCamera.name = "摄像头1";
            newCamera.cameraIP = newCameraIP.toStdString();
            m_configResult.cameras.push_back(newCamera);
            LOG_INFO("Camera IP configured: [%s]\n", newCameraIP.toStdString().c_str());
        }
    }

    // 保存参数到配置文件
    if (m_config) {
        BeltTearingConfigResult currentConfig = sdkToConfigParam(m_algorithmParam);
        // 保留相机配置
        currentConfig.cameras = m_configResult.cameras;
        currentConfig.servers = m_configResult.servers;
        currentConfig.debugParam = m_configResult.debugParam;
        currentConfig.projectType = m_configResult.projectType;
        currentConfig.serverPort = m_configResult.serverPort;
        // 保存队列处理参数
        currentConfig.queueProcessParam.maxQueueSize = m_maxQueueSize;
        currentConfig.queueProcessParam.generationInterval = m_generationInterval;

        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());
            // 更新内存中的配置
            m_configResult = currentConfig;
        } 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 cameraConfig;
        if (!m_configResult.cameras.empty()) {
            cameraConfig["cameraIP"] = QString::fromStdString(m_configResult.cameras[0].cameraIP);
            cameraConfig["cameraName"] = QString::fromStdString(m_configResult.cameras[0].name);
        }
        configObj["cameraConfig"] = cameraConfig;

        // 队列处理参数
        QJsonObject queueConfig;
        queueConfig["maxQueueSize"] = m_maxQueueSize;
        queueConfig["generationInterval"] = m_generationInterval;
        configObj["queueProcessParam"] = queueConfig;

        // 调试参数
        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;
        
        QString versionText = QString("%1_%2_%3")
                              .arg(BELT_TEARING_SERVER_VERSION_STRING)
                              .arg(BELT_TEARING_SERVER_VERSION_BUILD)
                              .arg(BUILD_TIME.c_str());

        // 创建响应对象
        QJsonObject responseObj;
        responseObj["command"] = "configResponse";
        responseObj["version"] = versionText;  // 添加版本信息
        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");

    int result = stopCamera();
    if (result != 0) {
        LOG_WARNING("Failed to stop camera during reset, error code: %d\n", result);
    }

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

    // 清空简化协议的报警数据
    if (m_pRobotProtocolSimplified) {
        int ret = m_pRobotProtocolSimplified->ClearAlarmData();
        if (ret != 0) {
            LOG_WARNING("Failed to clear simplified protocol alarm data during reset, error code: %d\n", ret);
        } else {
            LOG_INFO("Simplified protocol alarm data cleared during reset\n");
        }
    }

    // 清除TCP协议的历史最大撕裂数据
    if (m_tearingProtocol) {
        m_tearingProtocol->clearHistoryMaxData();
    }

    // 清除简化协议的历史最大撕裂数据
    m_historyMaxTearing = HistoryMaxTearingData();

    // 清除仿真数据计数器
    m_simulationTearIdCounter = 1000;
    m_simulationCallCount = 0;

    result = startCamera();
    if (result != 0) {
        LOG_WARNING("Failed to start camera after reset, error code: %d\n", result);
    }

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

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

    // 停止相机
    int cameraResult = stopCamera();
    if (cameraResult != SUCCESS) {
        LOG_ERROR("Failed to stop camera, error code: %d\n", cameraResult);
        // 继续执行其他停止操作，但返回相机错误码
    }

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

    LOG_INFO("Work stopped successfully\n");
    return cameraResult;  // 返回相机操作结果，成功时为SUCCESS
}

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

    // 清除TCP协议的历史最大撕裂数据
    if (m_tearingProtocol) {
        m_tearingProtocol->clearHistoryMaxData();
    }

    // 清除简化协议的历史最大撕裂数据
    m_historyMaxTearing = HistoryMaxTearingData();  // 重置历史最大值
    if (m_pRobotProtocolSimplified) {
        m_pRobotProtocolSimplified->ClearAlarmData();
        LOG_INFO("Cleared simplified protocol alarm data on start work\n");
    }

    // 清除仿真数据计数器
    m_simulationTearIdCounter = 1000;
    m_simulationCallCount = 0;
    LOG_INFO("Cleared simulation counters on start work\n");

    // 启动相机
    int cameraResult = startCamera();
    if (cameraResult != SUCCESS) {
        LOG_ERROR("Failed to start camera, error code: %d\n", cameraResult);
        return cameraResult;  // 返回相机错误码
    }

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

    LOG_INFO("Work started successfully\n");
    return SUCCESS;  // 成功
}

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 SUCCESS;  // 没有数据可处理
        }

        // 只保留最后的m_generationInterval大小的数据进行检测
        algorithmDataCache.reserve(m_generationInterval);
        int cacheSize = static_cast<int>(m_laserLineQueue.size()) - m_generationInterval;
        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);

    // 使用TearingTcpProtocol发送DETECT_RESULT消息（新协议）
    if (m_tearingProtocol && !allTearings.empty()) {
        m_tearingProtocol->sendDetectResult(allTearings, image);
    }

    return SUCCESS;  // 成功完成检测任务
}

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

    // 从配置中获取串口参数
    const SerialPortParam& serialParam = m_configResult.serialPortParam;

    // 如果配置中的串口名为空，使用平台默认值
    std::string portName = serialParam.portName;

    // 初始化Modbus-RTU主端，使用配置的参数
    int nRet = m_pModbusRTUMaster->Initialize(portName.c_str(), serialParam.baudRate, serialParam.parity,
        serialParam.dataBits, serialParam.stopBits);

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


// 发送温度数据给所有客户端
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");

    // 根据配置选择协议类型
    m_modbusTCPProtocolType = m_configResult.modbusTCPProtocol;

    int nRet = 0;

    if (m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified) {
        // 使用简化协议
        LOG_INFO("Using simplified ModbusTCP protocol\n");

        // 创建RobotProtocolSimplified实例
        if (nullptr == m_pRobotProtocolSimplified) {
            m_pRobotProtocolSimplified = new RobotProtocolSimplified();
        }

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

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

        // 设置复位回调
        m_pRobotProtocolSimplified->SetResetCallback([this]() {
            LOG_INFO("ModbusTCP simplified protocol: Reset command received\n");
            this->ResetDetect();
        });

    } else {
        // 使用标准协议
        LOG_INFO("Using standard ModbusTCP protocol\n");

        // 创建RobotProtocol实例
        if (nullptr == m_pRobotProtocol) {
            m_pRobotProtocol = new RobotProtocol();
        }

        // 初始化协议服务（使用端口502）
        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 (type: %s)\n",
             m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified ? "Simplified" : "Standard");
    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;  // 返回处理结果
}

// 发送检测结果到机械臂
/**
 * @brief 发送检测结果到机械臂
 * @param detectionResults 检测到的所有撕裂信息
 * 
 * 功能说明：
 * 1. 根据协议类型发送数据（简化协议或标准协议）
 * 2. 在简化协议中，使用ROI的宽度和高度中的较大值找出最大的撕裂区域
 * 3. 使用对角线长度作为撕裂大小的更准确表示
 */
void BeltTearingPresenter::SendDetectionResultToRobot(const std::vector<SSG_beltTearingInfo>& detectionResults)
{
    // 根据协议类型发送数据
    if (m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified) {
        // 简化协议：只发送历史最大撕裂信息（从开流开始的最大值）
        if (!m_pRobotProtocolSimplified) {
            LOG_WARNING("Simplified robot protocol not initialized, cannot send detection results\n");
            return;
        }

        // 更新历史最大值
        if (!detectionResults.empty()) {
            // 找出本次检测的最大撕裂
            auto maxTearIt = std::max_element(detectionResults.begin(), detectionResults.end(),
                [](const SSG_beltTearingInfo& a, const SSG_beltTearingInfo& b) {
                    // 计算撕裂区域的宽度和高度，取较大值
                    double widthA = a.roi.right - a.roi.left;
                    double lengthA = a.roi.bottom - a.roi.top;
                    double maxSizeA = widthA > lengthA ? widthA : lengthA;

                    double widthB = b.roi.right - b.roi.left;
                    double lengthB = b.roi.bottom - b.roi.top;
                    double maxSizeB = widthB > lengthB ? widthB : lengthB;

                    return maxSizeA < maxSizeB;
                });

            // 计算本次最大撕裂的尺寸
            double dWidth = maxTearIt->roi.right - maxTearIt->roi.left;
            double dLength = maxTearIt->roi.bottom - maxTearIt->roi.top;
            double diagonalLength = std::sqrt(dWidth * dWidth + dLength * dLength);
            uint16_t currentMaxLength = static_cast<uint16_t>(diagonalLength);
            uint16_t currentMaxWidth = static_cast<uint16_t>(maxTearIt->tearWidth);
            uint32_t currentMaxId = maxTearIt->tearID;

            // 更新历史最大值
            if (!m_historyMaxTearing.hasData || currentMaxLength > m_historyMaxTearing.maxLength) {
                m_historyMaxTearing.maxLength = currentMaxLength;
                m_historyMaxTearing.maxWidth = currentMaxWidth;
                m_historyMaxTearing.maxId = currentMaxId;
                m_historyMaxTearing.hasData = true;
            }
        }

        // 构造报警数据（使用历史最大值）
        RobotProtocolSimplified::TearingAlarmData alarmData;
        if (m_historyMaxTearing.hasData) {
            alarmData.alarmFlag = 1;  // 有撕裂报警
            alarmData.maxLength = m_historyMaxTearing.maxLength;
            alarmData.maxWidth = m_historyMaxTearing.maxWidth;
            alarmData.maxId = m_historyMaxTearing.maxId;
        } else {
            // 没有历史数据
            alarmData.alarmFlag = 0;
            alarmData.maxLength = 0;
            alarmData.maxWidth = 0;
            alarmData.maxId = 0;
        }

        // 发送报警数据
        m_pRobotProtocolSimplified->SetAlarmData(alarmData);

    } else {
        // 标准协议：发送完整的撕裂信息列表
        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);
    }
}