GrabBag/App/WheelMeasure/WheelMeasureApp/Presenter/Src/WheelMeasurePresenter.cpp

#include "WheelMeasurePresenter.h"
#include "PathManager.h"
#include "VrLog.h"
#include "VrError.h"

// wheelArchHeigthMeasure SDK
#include "wheelArchHeigthMeasure_Export.h"

// PointCloudImageUtils
#include "PointCloudImageUtils.h"

#include <QThread>
#include <QApplication>
#include <QDateTime>
#include <cmath>

WheelMeasurePresenter::WheelMeasurePresenter(QObject* parent)
    : BasePresenter(parent)
{
    // 创建配置接口
    IVrWheelMeasureConfig::CreateInstance(&m_config);
    LOG_INFO("ALGO_VERSION: %s \n", wd_wheelArchHeigthMeasureVersion());
}

WheelMeasurePresenter::~WheelMeasurePresenter()
{
    if (m_config) {
        delete m_config;
        m_config = nullptr;
    }
}

int WheelMeasurePresenter::InitApp()
{
    LOG_INFO("WheelMeasurePresenter::InitApp() called\n");

    SetWorkStatus(WorkStatus::InitIng);

    // 加载配置
    QString configPath = PathManager::GetInstance().GetConfigFilePath();
    if (!initializeConfig(configPath)) {
        LOG_ERROR("Failed to initialize config from: %s\n", configPath.toStdString().c_str());
        if (m_statusUpdate) {
            m_statusUpdate->OnErrorOccurred("配置文件加载失败");
        }
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    // 初始化相机
    if (!initializeCameras()) {
        LOG_ERROR("Failed to initialize cameras\n");
        if (m_statusUpdate) {
            m_statusUpdate->OnErrorOccurred("相机初始化失败");
        }
    }

    LOG_INFO("WheelMeasurePresenter::InitApp() completed\n");
    return SUCCESS;
}

int WheelMeasurePresenter::InitAlgoParams()
{
    LOG_DEBUG("Initializing algorithm parameters\n");
    // 算法参数已在配置加载时初始化
    return SUCCESS;
}

int WheelMeasurePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    LOG_INFO("ProcessAlgoDetection called, data lines: %zu\n", detectionDataCache.size());

    // 处理检测数据
    processScanData(detectionDataCache);

    return SUCCESS;
}

void WheelMeasurePresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
{
    LOG_INFO("Camera[%d] status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");

    if (!m_statusUpdate) return;

    // 从配置中获取相机名称（cameraIndex从1开始）
    QString cameraName;
    int enabledIndex = 0;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (cameraConfig.enabled) {
            enabledIndex++;
            if (enabledIndex == cameraIndex) {
                cameraName = QString::fromStdString(cameraConfig.name);
                break;
            }
        }
    }
    if (cameraName.isEmpty()) {
        cameraName = QString("Camera%1").arg(cameraIndex);
    }

    // 切换到主线程更新UI
    QMetaObject::invokeMethod(this, [this, cameraName, isConnected]() {
        if (!m_statusUpdate) return;
        if (isConnected) {
            m_statusUpdate->OnCameraConnected(cameraName);
            m_statusUpdate->OnDeviceStatusChanged(cameraName, static_cast<int>(DeviceStatus::Online));
        } else {
            m_statusUpdate->OnCameraDisconnected(cameraName);
            m_statusUpdate->OnDeviceStatusChanged(cameraName, static_cast<int>(DeviceStatus::Offline));
        }
    }, Qt::QueuedConnection);
}

void WheelMeasurePresenter::OnWorkStatusChanged(WorkStatus status)
{
    // 写入Modbus工作状态 (地址1)
    uint16_t statusValue = 0;
    switch (status) {
        case WorkStatus::InitIng:   statusValue = 1; break;
        case WorkStatus::Ready:     statusValue = 2; break;
        case WorkStatus::Working:   statusValue = 3; break;
        case WorkStatus::Detecting: statusValue = 3; break;
        case WorkStatus::Completed: statusValue = 4; break;
        case WorkStatus::Error:     statusValue = 5; break;
        default:                    statusValue = 0; break;
    }
    WriteModbusRegisters(1, &statusValue, 1);

    // 切换到主线程更新UI
    QMetaObject::invokeMethod(this, [this, status]() {
        if (m_statusUpdate) {
            m_statusUpdate->OnWorkStatusChanged(status);
        }
    }, Qt::QueuedConnection);
}

void WheelMeasurePresenter::OnCameraCountChanged(int count)
{
    if (!m_statusUpdate) return;

    QStringList cameraNames;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (cameraConfig.enabled) {
            cameraNames.append(QString::fromStdString(cameraConfig.name));
        }
    }
    LOG_INFO("OnCameraCountChanged: count=%d, cameraNames=%d\n", count, cameraNames.size());

    // 切换到主线程更新UI
    QMetaObject::invokeMethod(this, [this, cameraNames]() {
        if (m_statusUpdate) {
            m_statusUpdate->OnNeedShowImageCount(cameraNames);
        }
    }, Qt::QueuedConnection);
}

void WheelMeasurePresenter::OnStatusUpdate(const std::string& statusMessage)
{
    if (!m_statusUpdate) return;

    QString msg = QString::fromStdString(statusMessage);
    // 切换到主线程更新UI
    QMetaObject::invokeMethod(this, [this, msg]() {
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(msg);
        }
    }, Qt::QueuedConnection);
}

bool WheelMeasurePresenter::initializeConfig(const QString& configPath)
{
    if (!m_config) {
        LOG_ERROR("Config interface is null\n");
        return false;
    }

    m_configResult = m_config->LoadConfig(configPath.toStdString());

    // 设置配置改变通知
    m_config->SetConfigChangeNotify(this);

    LOG_INFO("Config loaded successfully, cameras: %zu\n", m_configResult.cameras.size());
    return true;
}

bool WheelMeasurePresenter::initializeCameras()
{
    // 转换相机配置为DeviceInfo列表
    std::vector<DeviceInfo> cameraList;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (!cameraConfig.enabled) {
            LOG_INFO("Camera %s is disabled, skipping\n", cameraConfig.name.c_str());
            continue;
        }

        DeviceInfo deviceInfo;
        deviceInfo.index = cameraConfig.cameraIndex;
        deviceInfo.name = cameraConfig.name;
        deviceInfo.ip = cameraConfig.cameraIP;
        cameraList.push_back(deviceInfo);
    }

    // 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=true）
    int result = InitCamera(cameraList, false, true);

    LOG_INFO("Camera initialization completed. Connected cameras: %zu\n", m_vrEyeDeviceList.size());

    return result == SUCCESS;
}

QStringList WheelMeasurePresenter::getCameraNames() const
{
    QStringList names;
    for (const auto& camera : m_vrEyeDeviceList) {
        names.append(QString::fromStdString(camera.first));
    }
    return names;
}

void WheelMeasurePresenter::ResetDetect(int cameraIndex)
{
    StopDetection();

    // 设置当前相机索引（从0-based转换为1-based）
    m_currentCameraIndex = cameraIndex + 1;
    LOG_INFO("ResetDetect: cameraIndex=%d, m_currentCameraIndex=%d\n", cameraIndex, m_currentCameraIndex);

    // 设置工作状态为检测中
    SetWorkStatus(WorkStatus::Working);

    // 清空数据
    ClearDetectionDataCache();

    // 注意：不调用 OnClearMeasureData()，因为 mainwindow 中的 onDeviceClicked
    // 已经调用了 clearDeviceResult(deviceName) 来清除指定设备的结果

    // 重新开始检测（BasePresenter::StartDetection 期望 1-based 索引）
    StartDetection(m_currentCameraIndex);
}

void WheelMeasurePresenter::StartAllDetection()
{
    LOG_INFO("Starting sequential detection for all cameras\n");

    // 计算启用的相机数量
    m_sequentialTotalCount = 0;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (cameraConfig.enabled) {
            m_sequentialTotalCount++;
        }
    }

    if (m_sequentialTotalCount == 0) {
        LOG_WARNING("No enabled cameras to detect\n");
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(QString("没有可用的相机设备"));
        }
        return;
    }

    // 初始化顺序检测状态
    m_sequentialDetecting = true;
    m_stopSequentialRequested = false;
    m_sequentialCurrentIndex = 0;

    // 清空之前的检测结果
    if (m_statusUpdate) {
        m_statusUpdate->OnClearMeasureData();
    }

    LOG_INFO("Sequential detection started, total cameras: %d\n", m_sequentialTotalCount);

    // 开始检测第一个设备
    continueSequentialDetection();
}

void WheelMeasurePresenter::StopAllDetection()
{
    LOG_INFO("Stop sequential detection requested\n");

    if (m_sequentialDetecting) {
        // 设置停止标志，等待当前设备检测完成后停止
        m_stopSequentialRequested = true;
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(QString("正在完成当前设备检测，之后将停止..."));
        }
    } else {
        // 如果不是顺序检测模式，直接停止
        StopDetection();
    }
}

void WheelMeasurePresenter::continueSequentialDetection()
{
    // 检查是否应该停止
    if (m_stopSequentialRequested) {
        LOG_INFO("Sequential detection stopped by user request\n");
        m_sequentialDetecting = false;
        m_stopSequentialRequested = false;
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(QString("顺序检测已停止"));
        }
        SetWorkStatus(WorkStatus::Ready);
        return;
    }

    // 检查是否还有设备需要检测
    if (m_sequentialCurrentIndex >= m_sequentialTotalCount) {
        LOG_INFO("Sequential detection completed, all %d cameras processed\n", m_sequentialTotalCount);
        m_sequentialDetecting = false;
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(QString("所有 %1 个设备检测完成").arg(m_sequentialTotalCount));
        }
        SetWorkStatus(WorkStatus::Completed);
        return;
    }

    // 获取当前要检测的相机索引（1-based）
    int cameraIndex = m_sequentialCurrentIndex + 1;
    m_currentCameraIndex = cameraIndex;

    // 获取相机名称
    QString cameraName;
    int enabledIndex = 0;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (cameraConfig.enabled) {
            enabledIndex++;
            if (enabledIndex == cameraIndex) {
                cameraName = QString::fromStdString(cameraConfig.name);
                break;
            }
        }
    }

    LOG_INFO("Starting detection for camera %d/%d: %s\n",
             m_sequentialCurrentIndex + 1, m_sequentialTotalCount,
             cameraName.toStdString().c_str());

    if (m_statusUpdate) {
        m_statusUpdate->OnStatusUpdate(QString("正在检测设备 %1/%2: %3")
            .arg(m_sequentialCurrentIndex + 1)
            .arg(m_sequentialTotalCount)
            .arg(cameraName));
    }

    // 清空数据缓存
    ClearDetectionDataCache();

    // 开始检测当前相机
    StartDetection(cameraIndex);
}

void WheelMeasurePresenter::OnConfigChanged(const WheelMeasureConfigResult& configResult)
{
    LOG_INFO("Config changed notification received\n");
    m_configResult = configResult;
    emit configUpdated();
}

void WheelMeasurePresenter::processScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    LOG_INFO("Processing scan data, lines: %zu\n", detectionDataCache.size());

    // 打印扫描数据统计信息
    int totalPoints = 0;
    for (const auto& linePair : detectionDataCache) {
        totalPoints += linePair.second.nPointCount;
    }
    LOG_INFO("Scan data statistics: %zu lines, %d total points\n", detectionDataCache.size(), totalPoints);

    // 打印调平参数
    LOG_INFO("========== PlaneCalib Parameters ==========\n");
    if (!m_configResult.planeCalibParams.empty()) {
        for (const auto& calibParam : m_configResult.planeCalibParams) {
            LOG_INFO("Camera[%d] %s: isCalibrated=%d, planeHeight=%.2f\n",
                     calibParam.cameraIndex, calibParam.cameraName.c_str(),
                     calibParam.isCalibrated ? 1 : 0, calibParam.planeHeight);
            LOG_INFO("  planeCalib: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n",
                     calibParam.planeCalib[0], calibParam.planeCalib[1], calibParam.planeCalib[2],
                     calibParam.planeCalib[3], calibParam.planeCalib[4], calibParam.planeCalib[5],
                     calibParam.planeCalib[6], calibParam.planeCalib[7], calibParam.planeCalib[8]);
            LOG_INFO("  invRMatrix: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n",
                     calibParam.invRMatrix[0], calibParam.invRMatrix[1], calibParam.invRMatrix[2],
                     calibParam.invRMatrix[3], calibParam.invRMatrix[4], calibParam.invRMatrix[5],
                     calibParam.invRMatrix[6], calibParam.invRMatrix[7], calibParam.invRMatrix[8]);
        }
    } else {
        LOG_INFO("No PlaneCalib parameters loaded!\n");
    }

    // 打印算法参数
    LOG_INFO("========== Algorithm Parameters ==========\n");
    LOG_INFO("CornerParam: minEndingGap=%.2f, minEndingGap_z=%.2f, scale=%.2f, cornerTh=%.2f, jumpCornerTh_1=%.2f, jumpCornerTh_2=%.2f\n",
             m_configResult.algorithmParams.cornerParam.minEndingGap,
             m_configResult.algorithmParams.cornerParam.minEndingGap_z,
             m_configResult.algorithmParams.cornerParam.scale,
             m_configResult.algorithmParams.cornerParam.cornerTh,
             m_configResult.algorithmParams.cornerParam.jumpCornerTh_1,
             m_configResult.algorithmParams.cornerParam.jumpCornerTh_2);

    LOG_INFO("LineSegParam: segGapTh_y=%.2f, segGapTh_z=%.2f, maxDist=%.2f\n",
             m_configResult.algorithmParams.lineSegParam.segGapTh_y,
             m_configResult.algorithmParams.lineSegParam.segGapTh_z,
             m_configResult.algorithmParams.lineSegParam.maxDist);

    LOG_INFO("OutlierFilterParam: continuityTh=%.2f, outlierTh=%.2f\n",
             m_configResult.algorithmParams.filterParam.continuityTh,
             m_configResult.algorithmParams.filterParam.outlierTh);

    LOG_INFO("TreeGrowParam: yDeviation_max=%.2f, zDeviation_max=%.2f, maxLineSkipNum=%d, maxSkipDistance=%.2f, minLTypeTreeLen=%.2f, minVTypeTreeLen=%.2f\n",
             m_configResult.algorithmParams.growParam.yDeviation_max,
             m_configResult.algorithmParams.growParam.zDeviation_max,
             m_configResult.algorithmParams.growParam.maxLineSkipNum,
             m_configResult.algorithmParams.growParam.maxSkipDistance,
             m_configResult.algorithmParams.growParam.minLTypeTreeLen,
             m_configResult.algorithmParams.growParam.minVTypeTreeLen);

    LOG_INFO("DebugParam: enableDebug=%d, saveDebugImage=%d, printDetailLog=%d\n",
             m_configResult.debugParam.enableDebug,
             m_configResult.debugParam.saveDebugImage,
             m_configResult.debugParam.printDetailLog);

    LOG_INFO("============================================\n");

    // ========== 调用 wheelArchHeigthMeasure SDK ==========

    // 1. 转换检测数据为SDK所需格式
    std::vector<std::vector<SVzNL3DPosition>> scanLines;
    int convertResult = m_dataLoader.ConvertToSVzNL3DPosition(detectionDataCache, scanLines);

    // 检查数据有效性
    if (convertResult != SUCCESS || scanLines.empty()) {
        LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
        return;
    }

    // 2. 准备算法参数
    SSG_cornerParam cornerParam;
    cornerParam.minEndingGap = m_configResult.algorithmParams.cornerParam.minEndingGap;
    cornerParam.minEndingGap_z = m_configResult.algorithmParams.cornerParam.minEndingGap_z;
    cornerParam.scale = m_configResult.algorithmParams.cornerParam.scale;
    cornerParam.cornerTh = m_configResult.algorithmParams.cornerParam.cornerTh;
    cornerParam.jumpCornerTh_1 = m_configResult.algorithmParams.cornerParam.jumpCornerTh_1;
    cornerParam.jumpCornerTh_2 = m_configResult.algorithmParams.cornerParam.jumpCornerTh_2;

    SSG_lineSegParam lineSegParam;
    lineSegParam.segGapTh_y = m_configResult.algorithmParams.lineSegParam.segGapTh_y;
    lineSegParam.segGapTh_z = m_configResult.algorithmParams.lineSegParam.segGapTh_z;
    lineSegParam.maxDist = m_configResult.algorithmParams.lineSegParam.maxDist;

    SSG_outlierFilterParam filterParam;
    filterParam.continuityTh = m_configResult.algorithmParams.filterParam.continuityTh;
    filterParam.outlierTh = m_configResult.algorithmParams.filterParam.outlierTh;

    SSG_treeGrowParam growParam;
    growParam.yDeviation_max = m_configResult.algorithmParams.growParam.yDeviation_max;
    growParam.zDeviation_max = m_configResult.algorithmParams.growParam.zDeviation_max;
    growParam.maxLineSkipNum = m_configResult.algorithmParams.growParam.maxLineSkipNum;
    growParam.maxSkipDistance = m_configResult.algorithmParams.growParam.maxSkipDistance;
    growParam.minLTypeTreeLen = m_configResult.algorithmParams.growParam.minLTypeTreeLen;
    growParam.minVTypeTreeLen = m_configResult.algorithmParams.growParam.minVTypeTreeLen;

    // 3. 准备调平参数（使用当前相机的调平参数）
    SSG_planeCalibPara groundCalibPara;
    memset(&groundCalibPara, 0, sizeof(groundCalibPara));

    // 初始化为单位矩阵
    groundCalibPara.planeCalib[0] = 1.0;
    groundCalibPara.planeCalib[4] = 1.0;
    groundCalibPara.planeCalib[8] = 1.0;
    groundCalibPara.invRMatrix[0] = 1.0;
    groundCalibPara.invRMatrix[4] = 1.0;
    groundCalibPara.invRMatrix[8] = 1.0;
    groundCalibPara.planeHeight = 0.0;

    // 查找当前相机的调平参数
    WheelCameraPlaneCalibParam* calibParam = getPlaneCalibParam(m_currentCameraIndex);
    if (calibParam && calibParam->isCalibrated) {
        for (int i = 0; i < 9; ++i) {
            groundCalibPara.planeCalib[i] = calibParam->planeCalib[i];
            groundCalibPara.invRMatrix[i] = calibParam->invRMatrix[i];
        }
        groundCalibPara.planeHeight = calibParam->planeHeight;
        LOG_INFO("Using calibrated plane parameters for camera %d\n", m_currentCameraIndex);
        LOG_INFO("  planeHeight: %.3f, errorCompensation: %.2f\n", calibParam->planeHeight, calibParam->errorCompensation);

        if(calibParam){
            // 计算调平使用的地面高度（加上该相机的误差补偿）
            double adjustedPlaneHeight = groundCalibPara.planeHeight + calibParam->errorCompensation;
            LOG_INFO("  adjustedPlaneHeight (with compensation): %.3f\n", adjustedPlaneHeight);

            for(size_t i = 0; i < scanLines.size(); i++){
                wd_horizonCamera_lineDataR(scanLines[i], calibParam->planeCalib, adjustedPlaneHeight);
            }
        }
    } else {
        LOG_WARN("No calibration data for camera %d, using default parameters\n", m_currentCameraIndex);
    }


    // 4. 调用算法
    int errCode = 0;
    LOG_INFO("Calling wd_wheelArchHeigthMeasure...\n");

    WD_wheelArchInfo wheelArchResult = wd_wheelArchHeigthMeasure(
        scanLines,
        cornerParam,
        lineSegParam,
        filterParam,
        growParam,
        groundCalibPara,
        &errCode);

    LOG_INFO("Algorithm completed with errCode=%d\n", errCode);

    // 5. 处理算法结果
    WheelMeasureResult result;

    // 从配置中获取当前相机名称
    QString cameraName;
    int enabledIndex = 0;
    for (const auto& cameraConfig : m_configResult.cameras) {
        if (cameraConfig.enabled) {
            enabledIndex++;
            if (enabledIndex == m_currentCameraIndex) {
                cameraName = QString::fromStdString(cameraConfig.name);
                break;
            }
        }
    }
    if (cameraName.isEmpty() && !m_vrEyeDeviceList.empty()) {
        cameraName = QString::fromStdString(m_vrEyeDeviceList[0].first);
    }
    if (cameraName.isEmpty()) {
        cameraName = QString("Camera%1").arg(m_currentCameraIndex);
    }

    result.cameraName = cameraName;
    result.aliasName = cameraName;
    result.bResultValid = (errCode == 0);

    if (errCode == 0) {
        LOG_INFO("========== Wheel Arch Measurement Result ==========\n");
        LOG_INFO("wheelArchPos: (%.3f, %.3f, %.3f)\n",
                 wheelArchResult.wheelArchPos.x,
                 wheelArchResult.wheelArchPos.y,
                 wheelArchResult.wheelArchPos.z);
        LOG_INFO("wheelUpPos: (%.3f, %.3f, %.3f)\n",
                 wheelArchResult.wheelUpPos.x,
                 wheelArchResult.wheelUpPos.y,
                 wheelArchResult.wheelUpPos.z);
        LOG_INFO("wheelDownPos: (%.3f, %.3f, %.3f)\n",
                 wheelArchResult.wheelDownPos.x,
                 wheelArchResult.wheelDownPos.y,
                 wheelArchResult.wheelDownPos.z);
        LOG_INFO("archToCenterHeigth: %.3f archToGroundHeigth: %.3f mm\n", wheelArchResult.archToCenterHeigth, wheelArchResult.archToGroundHeigth);
        LOG_INFO("==================================================\n");

        if (m_statusUpdate) {
            QString statusMsg = QString("轮眉高度: %1 mm, 到地面高度: %2 mm").arg(wheelArchResult.archToCenterHeigth).arg(wheelArchResult.archToGroundHeigth);
            m_statusUpdate->OnStatusUpdate(statusMsg);
        }

        // 填充测量结果数据
        WheelMeasureData measureData;
        measureData.archToCenterHeight = wheelArchResult.archToCenterHeigth;
        measureData.archToGroundHeight = wheelArchResult.archToGroundHeigth;
        measureData.wheelArchPosX = wheelArchResult.wheelArchPos.x;
        measureData.wheelArchPosY = wheelArchResult.wheelArchPos.y;
        measureData.wheelArchPosZ = wheelArchResult.wheelArchPos.z;
        measureData.wheelUpPosX = wheelArchResult.wheelUpPos.x;
        measureData.wheelUpPosY = wheelArchResult.wheelUpPos.y;
        measureData.wheelUpPosZ = wheelArchResult.wheelUpPos.z;
        measureData.wheelDownPosX = wheelArchResult.wheelDownPos.x;
        measureData.wheelDownPosY = wheelArchResult.wheelDownPos.y;
        measureData.wheelDownPosZ = wheelArchResult.wheelDownPos.z;
        measureData.timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss");
        result.result.push_back(measureData);

        // 使用 PointCloudImageUtils 生成带检测结果的图像
        result.image = PointCloudImageUtils::GenerateWheelArchImage(
            scanLines,
            wheelArchResult.wheelArchPos,
            wheelArchResult.wheelUpPos,
            wheelArchResult.wheelDownPos,
            wheelArchResult.archToCenterHeigth,
            true);
    } else {
        LOG_ERROR("Algorithm failed with errCode=%d\n", errCode);
        if (m_statusUpdate) {
            m_statusUpdate->OnStatusUpdate(QString("算法检测失败，错误码: %1").arg(errCode));
        }

        // 生成仅有点云的图像（无检测结果）
        SVzNL3DPoint emptyPoint = {0.0, 0.0, 0.0};
        result.image = PointCloudImageUtils::GenerateWheelArchImage(
            scanLines,
            emptyPoint,
            emptyPoint,
            emptyPoint,
            0.0,
            false);
    }

    result.bImageValid = !result.image.isNull();

    // ========== 写入检测结果到 Modbus (地址2-25) ==========
    // 辅助lambda: float转两个uint16_t (大端模式)
    auto floatToUint16 = [](float value, uint16_t& high, uint16_t& low) {
        uint32_t bits;
        memcpy(&bits, &value, sizeof(float));
        high = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
        low = static_cast<uint16_t>(bits & 0xFFFF);
    };

    uint16_t modbusData[24];
    memset(modbusData, 0, sizeof(modbusData));

    // 地址2: 设备序号
    modbusData[0] = static_cast<uint16_t>(m_currentCameraIndex);
    // 地址3: 结果有效
    modbusData[1] = (errCode == 0) ? 1 : 0;

    if (errCode == 0) {
        // 地址4-5: 轮眉高度（到中心）
        floatToUint16(static_cast<float>(wheelArchResult.archToCenterHeigth), modbusData[2], modbusData[3]);
        // 地址6-7: 轮眉X
        floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.x), modbusData[4], modbusData[5]);
        // 地址8-9: 轮眉Y
        floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.y), modbusData[6], modbusData[7]);
        // 地址10-11: 轮眉Z
        floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.z), modbusData[8], modbusData[9]);
        // 地址12-13: 上点X
        floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.x), modbusData[10], modbusData[11]);
        // 地址14-15: 上点Y
        floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.y), modbusData[12], modbusData[13]);
        // 地址16-17: 上点Z
        floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.z), modbusData[14], modbusData[15]);
        // 地址18-19: 下点X
        floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.x), modbusData[16], modbusData[17]);
        // 地址20-21: 下点Y
        floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.y), modbusData[18], modbusData[19]);
        // 地址22-23: 下点Z
        floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.z), modbusData[20], modbusData[21]);
        // 地址24-25: 到地面高度
        floatToUint16(static_cast<float>(wheelArchResult.archToGroundHeigth), modbusData[22], modbusData[23]);
    }

    WriteModbusRegisters(2, modbusData, 24);
    LOG_INFO("Modbus: 写入检测结果到地址2-25, 设备=%d, 有效=%d\n", m_currentCameraIndex, modbusData[1]);

    // 检测完成后清零"检测控制"（地址0）
    uint16_t zero = 0;
    WriteModbusRegisters(0, &zero, 1);

    // 直接调用回调，不使用信号槽
    if (m_statusUpdate) {
        m_statusUpdate->OnMeasureResult(result);
    }

    SetWorkStatus(WorkStatus::Completed);

    // 如果正在进行顺序检测，继续检测下一个设备
    if (m_sequentialDetecting) {
        m_sequentialCurrentIndex++;
        QMetaObject::invokeMethod(this, [this]() {
            continueSequentialDetection();
        }, Qt::QueuedConnection);
    }
}

WheelCameraPlaneCalibParam* WheelMeasurePresenter::getPlaneCalibParam(int cameraIndex)
{
    for (auto& param : m_configResult.planeCalibParams) {
        if (param.cameraIndex == cameraIndex) {
            return &param;
        }
    }
    return nullptr;
}

// ============ ICameraLevelCalculator 接口实现 ============

bool WheelMeasurePresenter::CalculatePlaneCalibration(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
    double planeCalib[9],
    double& planeHeight,
    double invRMatrix[9])
{
    LOG_INFO("CalculatePlaneCalibration called, scan lines: %zu\n", scanData.size());

    // TODO: 调用调平算法库计算调平参数
    // 暂时返回单位矩阵
    planeCalib[0] = 1.0; planeCalib[1] = 0.0; planeCalib[2] = 0.0;
    planeCalib[3] = 0.0; planeCalib[4] = 1.0; planeCalib[5] = 0.0;
    planeCalib[6] = 0.0; planeCalib[7] = 0.0; planeCalib[8] = 1.0;

    invRMatrix[0] = 1.0; invRMatrix[1] = 0.0; invRMatrix[2] = 0.0;
    invRMatrix[3] = 0.0; invRMatrix[4] = 1.0; invRMatrix[5] = 0.0;
    invRMatrix[6] = 0.0; invRMatrix[7] = 0.0; invRMatrix[8] = 1.0;

    planeHeight = 0.0;

    return true;
}

// ============ ICameraLevelResultSaver 接口实现 ============

bool WheelMeasurePresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
                                                 int cameraIndex, const QString& cameraName)
{
    LOG_INFO("SaveLevelingResults: Camera[%d] %s, planeHeight=%.2f\n",
             cameraIndex, cameraName.toStdString().c_str(), planeHeight);

    // 查找或创建调平参数
    WheelCameraPlaneCalibParam* param = getPlaneCalibParam(cameraIndex);
    if (!param) {
        WheelCameraPlaneCalibParam newParam;
        newParam.cameraIndex = cameraIndex;
        newParam.cameraName = cameraName.toStdString();
        m_configResult.planeCalibParams.push_back(newParam);
        param = &m_configResult.planeCalibParams.back();
    }

    // 更新调平参数
    param->isCalibrated = true;
    param->planeHeight = planeHeight;
    for (int i = 0; i < 9; ++i) {
        param->planeCalib[i] = planeCalib[i];
        param->invRMatrix[i] = invRMatrix[i];
    }

    // 保存配置到文件
    QString configPath = PathManager::GetInstance().GetConfigFilePath();
    bool success = m_config->SaveConfig(configPath.toStdString(), m_configResult);

    if (success) {
        LOG_INFO("Leveling results saved successfully\n");
    } else {
        LOG_ERROR("Failed to save leveling results\n");
    }

    return success;
}

bool WheelMeasurePresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
                                                 double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    LOG_INFO("LoadLevelingResults: Camera[%d] %s\n", cameraIndex, cameraName.toStdString().c_str());

    WheelCameraPlaneCalibParam* param = getPlaneCalibParam(cameraIndex);
    if (!param || !param->isCalibrated) {
        LOG_WARN("No calibration data found for camera %d\n", cameraIndex);
        return false;
    }

    planeHeight = param->planeHeight;
    for (int i = 0; i < 9; ++i) {
        planeCalib[i] = param->planeCalib[i];
        invRMatrix[i] = param->invRMatrix[i];
    }

    LOG_INFO("Leveling results loaded: planeHeight=%.2f\n", planeHeight);
    return true;
}

// ============ 静态相机状态回调函数 ============

void WheelMeasurePresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    // 从pInfoParam获取this指针，转换回WheelMeasurePresenter*类型
    WheelMeasurePresenter* pThis = reinterpret_cast<WheelMeasurePresenter*>(pInfoParam);
    if (pThis) {
        // 调用实例的非静态成员函数
        pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
    }
}

void WheelMeasurePresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
    LOG_DEBUG("[Camera Notify] received: status=%d\n", (int)eStatus);

    switch (eStatus) {
        case EVzDeviceWorkStatus::keDeviceWorkStatus_Offline:
        {
            LOG_WARNING("[Camera Notify] Camera device offline/disconnected\n");

            // 通知UI相机状态变更
            if (m_statusUpdate) {
                m_statusUpdate->OnCameraDisconnected(QString("Camera"));
                m_statusUpdate->OnStatusUpdate(QString("相机设备离线"));
            }
            break;
        }

        case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Reconnect:
        {
            LOG_INFO("[Camera Notify] Camera device reconnecting\n");

            if (m_statusUpdate) {
                m_statusUpdate->OnStatusUpdate(QString("相机设备重连中..."));
            }
            break;
        }

        case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Comming:
        {
            LOG_INFO("[Camera Notify] Camera device connected\n");

            if (m_statusUpdate) {
                m_statusUpdate->OnCameraConnected(QString("Camera"));
                m_statusUpdate->OnStatusUpdate(QString("相机设备已连接"));
            }
            break;
        }

        default:
            LOG_DEBUG("[Camera Notify] Unhandled status: %d\n", (int)eStatus);
            break;
    }
}

// ============ Modbus写寄存器回调处理 ============

void WheelMeasurePresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count)
{
    LOG_INFO("OnModbusWriteCallback: address=%d, count=%d\n", startAddress, count);

    if (!data || count == 0) {
        return;
    }

    // 地址0: 写1-4直接开始检测对应设备
    if (startAddress == 0) {
        int deviceIndex = data[0];

        // 检查设备索引是否有效 (1-4)
        if (deviceIndex >= 1 && deviceIndex <= 4 &&
            deviceIndex <= static_cast<int>(m_vrEyeDeviceList.size())) {
            LOG_INFO("Modbus: 开始检测设备 %d\n", deviceIndex);
            // 使用 QMetaObject::invokeMethod 在主线程执行
            QMetaObject::invokeMethod(this, [this, deviceIndex]() {
                ResetDetect(deviceIndex - 1);  // ResetDetect 期望 0-based 索引
            }, Qt::QueuedConnection);
        } else {
            LOG_WARNING("Modbus: 无效的设备索引: %d (有效范围: 1-%d)\n",
                       deviceIndex, static_cast<int>(m_vrEyeDeviceList.size()));
        }
    }
}