GrabBag/App/BeltTearing/BeltTearingServer/BeltTearingPresenter.cpp

#include "BeltTearingPresenter.h"
#include "PathManager.h"
#include "version.h"
#include "PointCloudImageUtils.h"
#include "ImageProcessingWorker.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 "VrLog.h"
#include "VrTimeUtils.h"

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

BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
    : QObject(parent)
    , m_tcpServer(nullptr)
    , m_cameraInitTimer(new QTimer(this))
    , m_port(0)
    , m_config(nullptr)
    , m_eyeDevice(nullptr)
    , m_cameraInitialized(false)
    , m_cameraDetecting(false)
    , m_lineCounter(0)
    , m_imageWorker(new ImageProcessingWorker(this))
{
    // 设置静态实例
    s_instance = this;

    // 打印版本信息
    LOG_INFO("Initializing %s %s\n", getProductName().toStdString().c_str(), getVersionString().toStdString().c_str());
    LOG_INFO("Build: %s\n", getBuildInfo().toStdString().c_str());

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

    // 连接工作线程信号（只连接图像生成信号）
    connect(m_imageWorker, &ImageProcessingWorker::imageGenerated, this, &BeltTearingPresenter::onImageGenerated);

    // 工作线程在构造函数中自动启动，无需手动调用start()

    // 创建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();
}

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

    // 停止工作线程
    if (m_imageWorker) {
        m_imageWorker->stop();
        // 析构函数会自动等待线程结束，无需手动调用wait()
        delete m_imageWorker;
        m_imageWorker = nullptr;
    }

    stopServer();
    stopCamera();

    // 清理激光线队列中的内存
    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;
    }
}

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;
    }
    
    // 判断文件类型
    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();
                processPointCloudData(&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);
        }

        // if(nIndex > 500){
        //     break;
        // }

        // std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }

    inputFile.close();
}

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 (!m_eyeDevice) {
        if (!initializeCamera()) {
            // 启动定时器持续重试初始化
            LOG_WARNING("Camera initialization failed, starting retry timer...\n");
            m_cameraInitTimer->start(5000); // 每5秒重试一次
            return;
        }
    }
    
    // 尝试打开设备
    int result = m_eyeDevice->OpenDevice(nullptr, 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_DEBUG("Camera started 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;
}

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;
    }
    
    LOG_INFO("Camera stopped\n");
}

void BeltTearingPresenter::onCameraInitTimer()
{
    LOG_DEBUG("Retrying camera initialization...\n");
    
    // 尝试重新初始化和连接相机
    if (!m_eyeDevice) {
        if (!initializeCamera()) {
            return; // 继续重试
        }
    }
    
    // 尝试连接相机
    int result = m_eyeDevice->OpenDevice(nullptr, 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");
        } 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_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->processPointCloudData(pLaserLinePoint);
}

void BeltTearingPresenter::processPointCloudData(const SVzLaserLineData* pLaserLine)
{
    if (!pLaserLine) return;
    if(pLaserLine->nPointCount <= 0) return;

    try {
        // 将激光线数据添加到队列（用于图像生成）
        addLaserLineToQueue(pLaserLine);

        // 调用SDK算法
        int errorCode = 0;


        if(!m_bInitAlgo){
            m_hLineWorkers.resize(pLaserLine->nPointCount);

            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(
                NULL,  //空扫描线，用于复位内部静态变量
                0,
                0,
                &errorCode,
                m_hLineWorkers,
                m_beltTearings_new,
                m_beltTearings_growing,
                m_beltTearings_ended,
                m_beltTearings_unknown,  //未判明，应用无需处理。
                m_algorithmParam);
                m_bInitAlgo = true;
        }

        // 使用SDK算法进行撕裂检测
        SVzNL3DLaserLine algorithmData;
        algorithmData.nTimeStamp = pLaserLine->llTimeStamp;
        algorithmData.p3DPosition = static_cast<SVzNL3DPosition*>(pLaserLine->p3DPoint);
        algorithmData.nPositionCnt = pLaserLine->nPointCount;

        sg_detectBeltTearing(
            &algorithmData,
            static_cast<int>(pLaserLine->llFrameIdx),
            algorithmData.nPositionCnt,
            &errorCode,
            m_hLineWorkers,
            m_beltTearings_new,
            m_beltTearings_growing,
            m_beltTearings_ended,
            m_beltTearings_unknown,
            m_algorithmParam
        );

        // 合并所有检测结果
        std::vector<SSG_beltTearingInfo> allResults;
        allResults.reserve(m_beltTearings_new.size() + m_beltTearings_growing.size() + m_beltTearings_ended.size());

        allResults.insert(allResults.end(), m_beltTearings_new.begin(), m_beltTearings_new.end());
        allResults.insert(allResults.end(), m_beltTearings_growing.begin(), m_beltTearings_growing.end());
        allResults.insert(allResults.end(), m_beltTearings_ended.begin(), m_beltTearings_ended.end());
        // allResults.insert(allResults.end(), m_beltTearings_unknown.begin(), m_beltTearings_unknown.end());

        LOG_DEBUG("line count : %d algo detect count: %d (new:%d, growing:%d, ended:%d, unknown:%d)[%d]\n",
                  algorithmData.nPositionCnt,
                  allResults.size(), m_beltTearings_new.size(), m_beltTearings_growing.size(),
                  m_beltTearings_ended.size(), m_beltTearings_unknown.size(), errorCode);

        for(auto& item : m_beltTearings_new){
            LOG_DEBUG("-----new %lld ID: %d st: %d depth: %f \n", pLaserLine->llFrameIdx, item.tearID, item.tearStatus, item.tearDepth);
        }

#if 1
        for(auto& item : m_beltTearings_ended){
            LOG_DEBUG("-----end %lld ID: %d st: %d depth: %f \n", pLaserLine->llFrameIdx, item.tearID, item.tearStatus, item.tearDepth);
        }
#endif

        // 发送检测结果
        if (!allResults.empty()) {
            sendTearingResults(allResults);
        }

    } catch (const std::exception& e) {
        LOG_ERROR("Error in point cloud data processing: %s\n", e.what());
    }
}

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);
        // tearingObj["tearLength"]    = QString::number(result.tearLength);
        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_DEBUG("Sent tearing results (%d bytes) to %d clients\n", package.size(), m_clients.size());
    } else {
        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());
}

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

QString BeltTearingPresenter::getVersionString()
{
    return QString(BELT_TEARING_SERVER_VERSION_STRING);
}

QString BeltTearingPresenter::getProductName()
{
    return QString(BELT_TEARING_SERVER_PRODUCT_NAME);
}

QString BeltTearingPresenter::getBuildInfo()
{
    return QString("%1 %2 built on %3 %4 for %5")
            .arg(BELT_TEARING_SERVER_BUILD_TYPE)
            .arg(BELT_TEARING_SERVER_VERSION_STRING)
            .arg(BELT_TEARING_SERVER_BUILD_DATE)
            .arg(BELT_TEARING_SERVER_BUILD_TIME)
            .arg(BELT_TEARING_SERVER_PLATFORM);
}

// 激光线队列管理方法实现
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_imageWorker && m_imageWorker->isProcessing()){
        return;
    }


    // 每到图像生成间隔，提交图像生成任务给工作线程
    if (m_lineCounter % IMAGE_GENERATION_INTERVAL == 0) {
        // 准备扫描线数据
        std::vector<std::vector<SVzNL3DPosition>> scanLines;

        // 在锁保护下复制数据
        {
            std::lock_guard<std::mutex> lock(m_queueMutex);
            scanLines.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);
                    scanLines.emplace_back(std::move(linePoints)); // 使用move避免拷贝
                }
            }
        }

        LOG_DEBUG("addLaserLineToQueue, line counter: %d, scanLines size: %d \n", m_lineCounter, scanLines.size());
        // 只提交图像生成任务给工作线程（不包含算法检测）
        if (m_imageWorker && !scanLines.empty()) {
            m_imageWorker->requestImageGeneration(scanLines);
        }
    }

}


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", 85)) {
            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_DEBUG("Sent point cloud image (%d bytes) to %d clients\n", packet.size(), m_clients.size());
        } else {
            LOG_WARNING("Failed to send image data to all clients\n");
        }

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

void BeltTearingPresenter::onImageGenerated(const QImage& image)
{
    sendImageToClients(image);
}

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