GrabBag/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp

#include "WorkpieceHolePresenter.h"
#include "VrError.h"
#include "VrLog.h"
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QString>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include <QtCore/QDateTime>
#include <cmath>
#include <algorithm>
#include <QImage>
#include <QThread>
#include <atomic>
#include <QJsonObject>
#include <QJsonArray>

#include "Version.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"
#include "SG_baseDataType.h"
#include "VrConvert.h"
#include "TCPServerProtocol.h"
#include "DetectPresenter.h"
#include "PathManager.h"
#include "workpieceHolePositioning_Export.h"  // 调平算法接口

// 配置变化监听器代理实现
void ConfigChangeListenerProxy::OnSystemConfigChanged(const SystemConfig& config)
{
    if (m_presenter) {
        LOG_INFO("ConfigChangeListenerProxy: config changed, reloading algorithm parameters\n");
        m_presenter->InitAlgoParams();
    }
}

WorkpieceHolePresenter::WorkpieceHolePresenter(QObject *parent)
    : BasePresenter(parent)
    , m_pConfigManager(nullptr)
    , m_pDetectPresenter(nullptr)
    , m_pTCPServer(nullptr)
    , m_bTCPConnected(false)
    , m_pPLCModbusClient(nullptr)
    , m_bPLCConnected(false)
    , m_bRobotConnected(false)
{
    // 基类已经创建了相机重连定时器和检测数据缓存
}

WorkpieceHolePresenter::~WorkpieceHolePresenter()
{
    // 基类会自动处理：相机重连定时器、算法检测线程、检测数据缓存、相机设备资源

    // 释放 PLC Modbus 客户端
    if (m_pPLCModbusClient) {
        m_pPLCModbusClient->Shutdown();
        delete m_pPLCModbusClient;
        m_pPLCModbusClient = nullptr;
    }

    // 释放ConfigManager
    if (m_pConfigManager) {
        m_pConfigManager->Shutdown();
        delete m_pConfigManager;
        m_pConfigManager = nullptr;
    }

    // 释放TCP服务器
    if (m_pTCPServer) {
        m_pTCPServer->Deinitialize();
        delete m_pTCPServer;
        m_pTCPServer = nullptr;
    }

    // 释放检测处理器
    if(m_pDetectPresenter)
    {
        delete m_pDetectPresenter;
        m_pDetectPresenter = nullptr;
    }
}

int WorkpieceHolePresenter::InitApp()
{
    LOG_DEBUG("Start APP Version: %s\n", WORKPIECEHOLE_FULL_VERSION_STRING);

    // 初始化连接状态
    SetWorkStatus(WorkStatus::InitIng);

    m_pDetectPresenter = new DetectPresenter();

    int nRet = SUCCESS;

    // 创建 ConfigManager 实例
    m_pConfigManager = new ConfigManager();
    if (!m_pConfigManager) {
        LOG_ERROR("Failed to create ConfigManager instance\n");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("配置管理器创建失败");
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    // 初始化 ConfigManager
    if (!m_pConfigManager->Initialize()) {
        LOG_ERROR("Failed to initialize ConfigManager\n");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("配置管理器初始化失败");
        return ERR_CODE(DEV_CONFIG_ERR);
    }

    // 注册配置变化监听器
    m_pConfigListener = std::make_shared<ConfigChangeListenerProxy>(this);
    m_pConfigManager->AddConfigChangeListener(m_pConfigListener);

    LOG_INFO("Configuration loaded successfully\n");

    // 获取配置结果
    ConfigResult configResult = m_pConfigManager->GetConfigResult();

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

    LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
             m_vrEyeDeviceList.size(), m_currentCameraIndex);

    // 初始化TCP服务器
    nRet = InitTCPServer();
    if (nRet != 0) {
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化失败");
        m_bTCPConnected = false;
    } else {
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化成功");
    }

    // 初始化 PLC Modbus 客户端
    nRet = InitPLCModbus();
    if (nRet != 0) {
        LOG_WARNING("PLC Modbus initialization failed, continuing without PLC communication\n");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("PLC通信初始化失败");
    } else {
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("PLC通信初始化成功");
    }

    if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("设备初始化完成");

    CheckAndUpdateWorkStatus();

    if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("配置初始化成功");

    return SUCCESS;
}

// 初始化算法参数（实现BasePresenter纯虚函数）
int WorkpieceHolePresenter::InitAlgoParams()
{
    LOG_DEBUG("initializing algorithm parameters\n");
    QString exePath = QCoreApplication::applicationFilePath();

    // 清空现有的手眼标定矩阵列表
    m_clibMatrixList.clear();

    // 从 ConfigManager 获取配置结果（包含手眼标定矩阵）
    ConfigResult configResult = m_pConfigManager->GetConfigResult();

    // 从 config.xml 加载手眼标定矩阵
    CalibMatrix calibMatrix;
    memcpy(calibMatrix.clibMatrix, configResult.handEyeCalibMatrix.matrix, sizeof(double) * 16);
    m_clibMatrixList.push_back(calibMatrix);

    LOG_INFO("Loaded hand-eye calibration matrix from config.xml:\n");
    QString clibMatrixStr;
    for (int i = 0; i < 4; ++i) {
        clibMatrixStr.clear();
        for (int j = 0; j < 4; ++j) {
            clibMatrixStr += QString::asprintf("%8.4f ", calibMatrix.clibMatrix[i * 4 + j]);
        }
        LOG_INFO("  %s\n", clibMatrixStr.toStdString().c_str());
    }

    LOG_INFO("Total loaded %zu hand-eye calibration matrices\n", m_clibMatrixList.size());

    const VrAlgorithmParams& xmlParams = configResult.algorithmParams;

    LOG_INFO("Loaded XML params - WorkpieceHole: type=%d, holeDiameter=%.1f, holeDist_L=%.1f, holeDist_W=%.1f\n",
             xmlParams.workpieceHoleParam.workpieceType,
             xmlParams.workpieceHoleParam.holeDiameter,
             xmlParams.workpieceHoleParam.holeDist_L,
             xmlParams.workpieceHoleParam.holeDist_W);
    LOG_INFO("Loaded XML params - Filter: continuityTh=%.1f, outlierTh=%.1f\n",
             xmlParams.filterParam.continuityTh, xmlParams.filterParam.outlierTh);
    LOG_INFO("Loaded XML params - LineSeg: distScale=%.1f, segGapTh_y=%.1f, segGapTh_z=%.1f\n",
             xmlParams.lineSegParam.distScale, xmlParams.lineSegParam.segGapTh_y, xmlParams.lineSegParam.segGapTh_z);

    LOG_INFO("Algorithm parameters initialized successfully\n");

    // 循环打印所有相机的调平参数（添加安全检查）
    LOG_INFO("Loading plane calibration parameters for all cameras:\n");

    if (!xmlParams.planeCalibParam.cameraCalibParams.empty()) {
        for (const auto& cameraParam : xmlParams.planeCalibParam.cameraCalibParams) {
            try {
                LOG_INFO("Camera %d (%s) calibration parameters:\n",
                         cameraParam.cameraIndex, cameraParam.cameraName.c_str());
                LOG_INFO("  Is calibrated: %s\n", cameraParam.isCalibrated ? "YES" : "NO");
                LOG_INFO("  Plane height: %.3f\n", cameraParam.planeHeight);
                LOG_INFO("  Plane calibration matrix:\n");
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[0], cameraParam.planeCalib[1], cameraParam.planeCalib[2]);
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[3], cameraParam.planeCalib[4], cameraParam.planeCalib[5]);
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[6], cameraParam.planeCalib[7], cameraParam.planeCalib[8]);
                LOG_INFO("  Inverse rotation matrix:\n");
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[0], cameraParam.invRMatrix[1], cameraParam.invRMatrix[2]);
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[3], cameraParam.invRMatrix[4], cameraParam.invRMatrix[5]);
                LOG_INFO("    [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[6], cameraParam.invRMatrix[7], cameraParam.invRMatrix[8]);
                LOG_INFO("  --------------------------------\n");
            } catch (const std::exception& e) {
                LOG_ERROR("Exception while printing camera calibration parameters: %s\n", e.what());
            } catch (...) {
                LOG_ERROR("Unknown exception while printing camera calibration parameters\n");
            }
        }
    } else {
        LOG_WARNING("No plane calibration parameters found in configuration\n");
    }


    return SUCCESS;
}

// 手眼标定矩阵管理方法实现
CalibMatrix WorkpieceHolePresenter::GetClibMatrix(int index) const
{
    CalibMatrix clibMatrix;

    double initClibMatrix[16] = {
        1.0, 0.0, 0.0, 0.0,  // 第一行
        0.0, 1.0, 0.0, 0.0,  // 第二行
        0.0, 0.0, 1.0, 0.0,  // 第三行
        0.0, 0.0, 0.0, 1.0   // 第四行
    };
    memcpy(clibMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));

    if (index >= 0 && index < static_cast<int>(m_clibMatrixList.size())) {
        clibMatrix = m_clibMatrixList[index];
        memcpy(clibMatrix.clibMatrix, m_clibMatrixList[index].clibMatrix, sizeof(initClibMatrix));
    } else {
        LOG_WARNING("Invalid hand-eye calibration matrix\n");
    }

    return clibMatrix;
}

// ============ 坐标转换实现 ============

void WorkpieceHolePresenter::ConvertWorkpieceToRobotPose(const WD_workpieceInfo& workpieceInfo,
                                                          const CTRobotPose& robotPose,
                                                          const CTSixAxisCalibResult& handEye,
                                                          CTRobotPose& targetPose)
{
    // 1. 从方向向量构建旋转矩阵
    CTRotationMatrix rotMatrix;
    rotMatrix.at(0, 0) = workpieceInfo.x_dir.x;
    rotMatrix.at(1, 0) = workpieceInfo.x_dir.y;
    rotMatrix.at(2, 0) = workpieceInfo.x_dir.z;
    rotMatrix.at(0, 1) = workpieceInfo.y_dir.x;
    rotMatrix.at(1, 1) = workpieceInfo.y_dir.y;
    rotMatrix.at(2, 1) = workpieceInfo.y_dir.z;
    rotMatrix.at(0, 2) = workpieceInfo.z_dir.x;
    rotMatrix.at(1, 2) = workpieceInfo.z_dir.y;
    rotMatrix.at(2, 2) = workpieceInfo.z_dir.z;

    // 2. 将旋转矩阵转换为欧拉角
    CTEulerAngles eulerAngles = CCoordinateTransform::rotationMatrixToEuler(rotMatrix, CTEulerOrder::sZYX);

    // 3. 构造相机坐标系下的目标位姿
    CTCameraPose cameraPose(
        workpieceInfo.center.x,
        workpieceInfo.center.y,
        workpieceInfo.center.z,
        eulerAngles.roll,
        eulerAngles.pitch,
        eulerAngles.yaw
    );

    // 4. 使用六轴手眼转换（Eye-in-Hand模式）计算机器人目标位姿
    CCoordinateTransform::sixAxisEyeInHandCalcGraspPose(cameraPose, robotPose, handEye, targetPose);

    LOG_DEBUG("ConvertWorkpieceToRobotPose: Camera(%f, %f, %f, %f, %f, %f) -> Robot(%f, %f, %f, %f, %f, %f)\n",
              cameraPose.x, cameraPose.y, cameraPose.z,
              cameraPose.rx, cameraPose.ry, cameraPose.rz,
              targetPose.x, targetPose.y, targetPose.z,
              targetPose.rx, targetPose.ry, targetPose.rz);
}

void WorkpieceHolePresenter::ConvertWorkpieceToRobotPoseEyeToHand(const WD_workpieceInfo& workpieceInfo,
                                                                   const CTSixAxisCalibResult& cameraToBase,
                                                                   CTRobotPose& targetPose)
{
    // 1. 从方向向量构建旋转矩阵
    CTRotationMatrix rotMatrix;
    rotMatrix.at(0, 0) = workpieceInfo.x_dir.x;
    rotMatrix.at(1, 0) = workpieceInfo.x_dir.y;
    rotMatrix.at(2, 0) = workpieceInfo.x_dir.z;
    rotMatrix.at(0, 1) = workpieceInfo.y_dir.x;
    rotMatrix.at(1, 1) = workpieceInfo.y_dir.y;
    rotMatrix.at(2, 1) = workpieceInfo.y_dir.z;
    rotMatrix.at(0, 2) = workpieceInfo.z_dir.x;
    rotMatrix.at(1, 2) = workpieceInfo.z_dir.y;
    rotMatrix.at(2, 2) = workpieceInfo.z_dir.z;

    // 2. 将旋转矩阵转换为欧拉角
    CTEulerAngles eulerAngles = CCoordinateTransform::rotationMatrixToEuler(rotMatrix, CTEulerOrder::sZYX);

    // 3. 构造相机坐标系下的目标位姿
    CTCameraPose cameraPose(
        workpieceInfo.center.x,
        workpieceInfo.center.y,
        workpieceInfo.center.z,
        eulerAngles.roll,
        eulerAngles.pitch,
        eulerAngles.yaw
    );

    // 4. 使用六轴手眼转换（Eye-to-Hand模式）计算机器人目标位姿
    CCoordinateTransform::sixAxisEyeToHandCalcGraspPose(cameraPose, cameraToBase, targetPose);

    LOG_DEBUG("ConvertWorkpieceToRobotPoseEyeToHand: Camera(%f, %f, %f, %f, %f, %f) -> Robot(%f, %f, %f, %f, %f, %f)\n",
              cameraPose.x, cameraPose.y, cameraPose.z,
              cameraPose.rx, cameraPose.ry, cameraPose.rz,
              targetPose.x, targetPose.y, targetPose.z,
              targetPose.rx, targetPose.ry, targetPose.rz);
}


void WorkpieceHolePresenter::CheckAndUpdateWorkStatus()
{
    if (m_bCameraConnected) {
        SetWorkStatus(WorkStatus::Ready);
    } else {
        SetWorkStatus(WorkStatus::Error);
    }
}

// 实现BasePresenter纯虚函数：执行算法检测
int WorkpieceHolePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
{
    LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");

    // 1. 获取缓存的点云数据（已由基类验证非空）
    unsigned int lineNum = detectionDataCache.size();
    if(GetStatusCallback<IYWorkpieceHoleStatus>()){
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("扫描线数:" + std::to_string(lineNum) + "，正在算法检测...");
    }

    // 检查检测处理器是否已初始化
    if (!m_pDetectPresenter) {
        LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
        if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
            if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("检测处理器未初始化");
        }
        return ERR_CODE(DEV_NOT_FIND);
    }

    CVrTimeUtils oTimeUtils;

    // 获取当前使用的手眼标定矩阵
    const CalibMatrix currentClibMatrix = GetClibMatrix(m_currentCameraIndex - 1);

    // 打印手眼标定矩阵
    LOG_INFO("[Algo Thread] Hand-Eye Calibration Matrix (Camera %d):\n", m_currentCameraIndex);
    LOG_INFO("  [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[0], currentClibMatrix.clibMatrix[1], currentClibMatrix.clibMatrix[2], currentClibMatrix.clibMatrix[3]);
    LOG_INFO("  [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[4], currentClibMatrix.clibMatrix[5], currentClibMatrix.clibMatrix[6], currentClibMatrix.clibMatrix[7]);
    LOG_INFO("  [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[8], currentClibMatrix.clibMatrix[9], currentClibMatrix.clibMatrix[10], currentClibMatrix.clibMatrix[11]);
    LOG_INFO("  [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[12], currentClibMatrix.clibMatrix[13], currentClibMatrix.clibMatrix[14], currentClibMatrix.clibMatrix[15]);

    // 从 ConfigManager 获取算法参数和调试参数
    VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    VrDebugParam debugParam = configResult.debugParam;

    // 获取旋转顺序配置
    int eulerOrder = configResult.handEyeCalibMatrix.eulerOrder;
    LOG_INFO("[Algo Thread] Using euler order: %d\n", eulerOrder);

    // 获取方向向量反向配置
    int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
    LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);

    WorkpieceHoleDetectionResult detectionResult;

    int nRet = m_pDetectPresenter->DetectWorkpieceHole(m_currentCameraIndex, detectionDataCache,
                                algorithmParams, debugParam, m_dataLoader,
                                currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult);
    // 根据项目类型选择处理方式
    if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
        QString err = QString("错误:%1").arg(nRet);
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate(QString("检测%1").arg(SUCCESS == nRet ? "成功": err).toStdString());
    }

    LOG_INFO("[Algo Thread] wd_workpieceHolePositioning detected %zu workpieces time : %.2f ms\n", detectionResult.positions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
    ERR_CODE_RETURN(nRet);

    // 8. 通知UI检测结果
    detectionResult.cameraIndex = m_currentCameraIndex;
    if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
        pStatus->OnDetectionResult(detectionResult);
    }

    // 更新状态
    QString statusMsg = QString("检测完成，发现%1个工件").arg(detectionResult.positions.size());
    if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());

    // 发送检测结果给TCP客户端
    _SendDetectionResultToTCP(detectionResult, m_currentCameraIndex);

    // 发送坐标数据到 PLC/机械臂
    SendCoordinateDataToPLC(detectionResult);

    // 9. 检测完成后，将工作状态更新为"完成"
    SetWorkStatus(WorkStatus::Completed);

    // 恢复到就绪状态
    SetWorkStatus(WorkStatus::Ready);

    return SUCCESS;
}


// 实现配置改变通知接口
void WorkpieceHolePresenter::OnConfigChanged(const ConfigResult& configResult)
{
    LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");

    // 重新初始化算法参数
    int result = InitAlgoParams();
    if (result == SUCCESS) {
        LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
        if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
            if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
        }
    } else {
        LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
        if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
            if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
        }
    }
}

// 根据相机索引获取调平参数
SSG_planeCalibPara WorkpieceHolePresenter::_GetCameraCalibParam(int cameraIndex)
{
    // 查找指定相机索引的调平参数
    SSG_planeCalibPara calibParam;

    // 使用单位矩阵（未校准状态）
    double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
    for (int i = 0; i < 9; i++) {
        calibParam.planeCalib[i] = identityMatrix[i];
        calibParam.invRMatrix[i] = identityMatrix[i];
    }
    calibParam.planeHeight = -1.0;  // 使用默认高度

    // 从 ConfigManager 获取算法参数
    VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();

    for (const auto& cameraParam : algorithmParams.planeCalibParam.cameraCalibParams) {
        if (cameraParam.cameraIndex == cameraIndex) {

            // 根据isCalibrated标志决定使用标定矩阵还是单位矩阵
            if (cameraParam.isCalibrated) {
                // 使用实际的标定矩阵
                for (int i = 0; i < 9; i++) {
                    calibParam.planeCalib[i] = cameraParam.planeCalib[i];
                    calibParam.invRMatrix[i] = cameraParam.invRMatrix[i];
                }
                calibParam.planeHeight = cameraParam.planeHeight;
            }
        }
    }
    return calibParam;
}

// 实现BasePresenter纯虚函数：相机状态变化通知
void WorkpieceHolePresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
{
    LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");

    // 通知UI更新相机状态
    if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
        if (cameraIndex == 1) {
            if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnCamera1StatusChanged(isConnected);
        } else if (cameraIndex == 2) {
            if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnCamera2StatusChanged(isConnected);
        }

        // 获取相机名称用于状态消息
        QString cameraName;
        int arrayIndex = cameraIndex - 1;
        if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
            cameraName = QString::fromStdString(m_vrEyeDeviceList[arrayIndex].first);
        } else {
            cameraName = QString("相机%1").arg(cameraIndex);
        }

        QString statusMsg = QString("%1%2").arg(cameraName).arg(isConnected ? "已连接" : "已断开");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
    }

    // 检查并更新工作状态
    CheckAndUpdateWorkStatus();
}

// 实现BasePresenter虚函数：工作状态变化通知
void WorkpieceHolePresenter::OnWorkStatusChanged(WorkStatus status)
{
    auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>();
    if (pStatus) {
        pStatus->OnWorkStatusChanged(status);
    }
}


// 实现BasePresenter虚函数：相机数量变化通知
void WorkpieceHolePresenter::OnCameraCountChanged(int count)
{
    auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>();
    if (pStatus) {
        pStatus->OnCameraCountChanged(count);
    }
}

// 实现BasePresenter虚函数：状态文字更新通知
void WorkpieceHolePresenter::OnStatusUpdate(const std::string& statusMessage)
{
    auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>();
    if (pStatus) {
        pStatus->OnStatusUpdate(statusMessage);
    }
}

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

bool WorkpieceHolePresenter::CalculatePlaneCalibration(
    const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
    double planeCalib[9],
    double& planeHeight,
    double invRMatrix[9])
{
    try {
        // 检查是否有足够的扫描数据
        if (scanData.empty()) {
            LOG_ERROR("No scan data available for plane calibration\n");
            return false;
        }

        LOG_INFO("Calculating plane calibration from %zu scan lines\n", scanData.size());

        // 转换为算法需要的XYZ格式
        LaserDataLoader dataLoader;
        std::vector<std::vector<SVzNL3DPosition>> xyzData;
        int convertResult = dataLoader.ConvertToSVzNL3DPosition(scanData, xyzData);
        if (convertResult != SUCCESS || xyzData.empty()) {
            LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
            return false;
        }

        // 调用工件孔定位项目的调平算法
        SSG_planeCalibPara calibResult = wd_getGroundCalibPara(xyzData);

        // 打印算法返回结果
        LOG_INFO("=== wd_getGroundCalibPara 算法结果 ===\n");
        LOG_INFO("planeHeight: %.6f\n", calibResult.planeHeight);
        LOG_INFO("planeCalib (调平旋转矩阵):\n");
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[0], calibResult.planeCalib[1], calibResult.planeCalib[2]);
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[3], calibResult.planeCalib[4], calibResult.planeCalib[5]);
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[6], calibResult.planeCalib[7], calibResult.planeCalib[8]);
        LOG_INFO("invRMatrix (逆旋转矩阵):\n");
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[0], calibResult.invRMatrix[1], calibResult.invRMatrix[2]);
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[3], calibResult.invRMatrix[4], calibResult.invRMatrix[5]);
        LOG_INFO("  [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[6], calibResult.invRMatrix[7], calibResult.invRMatrix[8]);
        LOG_INFO("========================================\n");

        // 将结构体中的数据复制到输出参数
        for (int i = 0; i < 9; i++) {
            planeCalib[i] = calibResult.planeCalib[i];
            invRMatrix[i] = calibResult.invRMatrix[i];
        }
        planeHeight = calibResult.planeHeight;

        LOG_INFO("Plane calibration calculated successfully: height=%.3f\n", planeHeight);
        return true;

    } catch (const std::exception& e) {
        LOG_ERROR("Exception in CalculatePlaneCalibration: %s\n", e.what());
        return false;
    } catch (...) {
        LOG_ERROR("Unknown exception in CalculatePlaneCalibration\n");
        return false;
    }
}

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

bool WorkpieceHolePresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
                                              int cameraIndex, const QString& cameraName)
{
    try {
        if (!m_pConfigManager) {
            LOG_ERROR("ConfigManager is null, cannot save leveling results\n");
            return false;
        }

        // 验证传入的相机参数
        if (cameraIndex <= 0) {
            LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
            return false;
        }

        if (cameraName.isEmpty()) {
            LOG_ERROR("Camera name is empty\n");
            return false;
        }

        // 获取当前配置
        QString configPath = PathManager::GetInstance().GetConfigFilePath();
        LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());

        SystemConfig systemConfig = m_pConfigManager->GetConfig();

        // 创建或更新指定相机的调平参数
        VrCameraPlaneCalibParam cameraParam;
        cameraParam.cameraIndex = cameraIndex;
        cameraParam.cameraName = cameraName.toStdString();
        cameraParam.planeHeight = planeHeight;
        cameraParam.isCalibrated = true;

        // 复制校准矩阵
        for (int i = 0; i < 9; i++) {
            cameraParam.planeCalib[i] = planeCalib[i];
            cameraParam.invRMatrix[i] = invRMatrix[i];
        }

        // 更新配置中的相机校准参数
        systemConfig.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);

        // 更新并保存配置
        if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
            LOG_ERROR("Failed to update config with leveling results\n");
            return false;
        }

        if (!m_pConfigManager->SaveConfigToFile(configPath.toStdString())) {
            LOG_ERROR("Failed to save config file with leveling results\n");
            return false;
        }

        LOG_INFO("Leveling results saved successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
        LOG_INFO("Plane height: %.3f\n", planeHeight);
        LOG_INFO("Calibration marked as completed\n");

        return true;

    } catch (const std::exception& e) {
        LOG_ERROR("Exception in SaveLevelingResults: %s\n", e.what());
        return false;
    }
}

bool WorkpieceHolePresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
                                              double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
    try {
        if (!m_pConfigManager) {
            LOG_ERROR("ConfigManager is null, cannot load calibration data\n");
            return false;
        }

        // 从ConfigManager获取配置结果
        ConfigResult configResult = m_pConfigManager->GetConfigResult();

        // 获取指定相机的标定参数
        VrCameraPlaneCalibParam cameraParamValue;
        if (!configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraParamValue) || !cameraParamValue.isCalibrated) {
            LOG_INFO("No calibration data found for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
            return false;
        }

        // 复制标定数据
        for (int i = 0; i < 9; i++) {
            planeCalib[i] = cameraParamValue.planeCalib[i];
            invRMatrix[i] = cameraParamValue.invRMatrix[i];
        }
        planeHeight = cameraParamValue.planeHeight;

        LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
        LOG_INFO("Plane height: %.3f\n", planeHeight);

        return true;

    } catch (const std::exception& e) {
        LOG_ERROR("Exception in LoadLevelingResults: %s\n", e.what());
        return false;
    }
}

// ============ PLC Modbus 相关实现 ============

int WorkpieceHolePresenter::InitPLCModbus()
{
    LOG_INFO("Initializing PLC Modbus client\n");

    // 从配置获取 PLC 和机械臂的 IP 地址
    ConfigResult configResult = m_pConfigManager->GetConfigResult();

    // 从配置文件获取 PLC 配置
    std::string plcIP = configResult.plcRobotServerConfig.plcServerIp;
    int plcPort = configResult.plcRobotServerConfig.plcServerPort;

    // 检查是否配置了 PLC 通信（如果为空则跳过初始化）
    if (plcIP.empty()) {
        LOG_INFO("PLC IP not configured, skipping PLC Modbus initialization\n");
        return SUCCESS;
    }

    // 创建 PLCModbusClient 实例
    m_pPLCModbusClient = new PLCModbusClient();

    // 设置回调函数
    m_pPLCModbusClient->SetPhotoTriggerCallback([this](int cameraIndex) {
        OnPLCPhotoRequested(cameraIndex);
    });

    m_pPLCModbusClient->SetConnectionStateCallback([this](bool plcConnected) {
        m_bPLCConnected = plcConnected;
        LOG_INFO("PLC connection state changed: PLC=%s\n",
                 plcConnected ? "connected" : "disconnected");

        // 通知 UI 更新连接状态（PLC连接状态关联到机械臂状态指示器）
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            pStatus->OnRobotConnectionChanged(plcConnected);
            std::string statusMsg = std::string("PLC:") + (plcConnected ? "已连接" : "断开");
            pStatus->OnStatusUpdate(statusMsg);
        }
    });

    m_pPLCModbusClient->SetErrorCallback([this](const std::string& errorMsg) {
        LOG_ERROR("PLC Modbus error: %s\n", errorMsg.c_str());
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            pStatus->OnStatusUpdate(std::string("PLC错误: ") + errorMsg);
        }
    });

    // 设置重连回调，通知用户正在尝试重连
    m_pPLCModbusClient->SetReconnectingCallback([this](const std::string& device, int attempt) {
        LOG_INFO("Attempting to reconnect %s (attempt %d)\n", device.c_str(), attempt);
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            std::string statusMsg = device + "正在重连(第" + std::to_string(attempt) + "次)...";
            pStatus->OnStatusUpdate(statusMsg);
        }
    });

    // 构建寄存器地址配置
    PLCModbusClient::RegisterConfig regConfig;
    regConfig.addrPhotoRequest = configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest;
    regConfig.addrDataComplete = configResult.plcRobotServerConfig.registerConfig.addrDataComplete;
    regConfig.addrCoordDataStart = configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart;
    regConfig.byteOrder = configResult.plcRobotServerConfig.byteOrder;

    // 初始化连接
    bool initResult = m_pPLCModbusClient->Initialize(plcIP, plcPort, regConfig);

    if (!initResult) {
        LOG_ERROR("Failed to initialize PLC Modbus client\n");
        return ERR_CODE(DEV_OPEN_ERR);
    }

    // 启动轮询（100ms 间隔）
    m_pPLCModbusClient->StartPolling(100);

    LOG_INFO("PLC Modbus client initialized successfully\n");
    return SUCCESS;
}

void WorkpieceHolePresenter::OnPLCPhotoRequested(int cameraIndex)
{
    LOG_INFO("PLC photo request received for camera %d\n", cameraIndex);

    // 暂停拍照请求轮询（检测期间不再读取 D1000）
    if (m_pPLCModbusClient) {
        m_pPLCModbusClient->PausePhotoRequestPolling();
    }

    // 清除 PLC 的拍照请求标志（D1000 = 0）
    if (m_pPLCModbusClient) {
        m_pPLCModbusClient->ClearPhotoRequest();
    }

    // 设置当前相机索引
    SetDefaultCameraIndex(cameraIndex);

    // 触发检测（使用基类的 StartDetection 方法）
    int nRet = StartDetection(cameraIndex, false);  // 非自动模式
    if (nRet != SUCCESS) {
        LOG_ERROR("Failed to trigger detection from PLC request, error: %d\n", nRet);
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            pStatus->OnStatusUpdate("PLC触发检测失败");
        }
        // 检测失败时也要恢复轮询
        if (m_pPLCModbusClient) {
            m_pPLCModbusClient->ResumePhotoRequestPolling();
        }
    }
}

void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectionResult& result)
{
    if (!m_pPLCModbusClient) {
        LOG_WARNING("PLC Modbus client not initialized, cannot send coordinate data\n");
        return;
    }

    if (!m_pPLCModbusClient->IsPLCConnected()) {
        LOG_WARNING("PLC not connected, cannot send coordinate data\n");
        // 恢复轮询
        m_pPLCModbusClient->ResumePhotoRequestPolling();
        return;
    }

    // 检查检测结果（errorCode == 0 表示成功）
    if (result.errorCode != 0) {
        LOG_WARNING("Invalid detection result, skipping coordinate send\n");
        // 恢复轮询
        m_pPLCModbusClient->ResumePhotoRequestPolling();
        return;
    }

    // 检查是否有工件位置数据
    if (result.positions.empty()) {
        LOG_WARNING("No workpiece positions found, skipping coordinate send\n");
        // 恢复轮询
        m_pPLCModbusClient->ResumePhotoRequestPolling();
        return;
    }

    // 获取姿态输出顺序配置
    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder;
    LOG_INFO("Using pose output order: %d\n", poseOutputOrder);

    // 只发送第一个工件的坐标数据
    const auto& pos = result.positions[0];
    PLCModbusClient::CoordinateData coord;

    // 使用 float 类型存储坐标数据
    coord.x = static_cast<float>(pos.x);
    coord.y = static_cast<float>(pos.y);
    coord.z = static_cast<float>(pos.z);

    // 根据姿态输出顺序配置调整输出
    switch (poseOutputOrder) {
        case POSE_ORDER_RPY:  // Roll, Pitch, Yaw（默认）
            coord.roll = static_cast<float>(pos.roll);
            coord.pitch = static_cast<float>(pos.pitch);
            coord.yaw = static_cast<float>(pos.yaw);
            break;
        case POSE_ORDER_RYP:  // Roll, Yaw, Pitch
            coord.roll = static_cast<float>(pos.roll);
            coord.pitch = static_cast<float>(pos.yaw);
            coord.yaw = static_cast<float>(pos.pitch);
            break;
        case POSE_ORDER_PRY:  // Pitch, Roll, Yaw
            coord.roll = static_cast<float>(pos.pitch);
            coord.pitch = static_cast<float>(pos.roll);
            coord.yaw = static_cast<float>(pos.yaw);
            break;
        case POSE_ORDER_PYR:  // Pitch, Yaw, Roll
            coord.roll = static_cast<float>(pos.pitch);
            coord.pitch = static_cast<float>(pos.yaw);
            coord.yaw = static_cast<float>(pos.roll);
            break;
        case POSE_ORDER_YRP:  // Yaw, Roll, Pitch
            coord.roll = static_cast<float>(pos.yaw);
            coord.pitch = static_cast<float>(pos.roll);
            coord.yaw = static_cast<float>(pos.pitch);
            break;
        case POSE_ORDER_YPR:  // Yaw, Pitch, Roll
            coord.roll = static_cast<float>(pos.yaw);
            coord.pitch = static_cast<float>(pos.pitch);
            coord.yaw = static_cast<float>(pos.roll);
            break;
        default:  // 默认 RPY
            coord.roll = static_cast<float>(pos.roll);
            coord.pitch = static_cast<float>(pos.pitch);
            coord.yaw = static_cast<float>(pos.yaw);
            break;
    }

    LOG_INFO("Workpiece: X=%.5f, Y=%.5f, Z=%.5f, R=%.5f, P=%.5f, Y=%.5f (order=%d)\n",
             coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw, poseOutputOrder);

    LOG_INFO("Sending workpiece position to PLC\n");

    // 发送坐标数据到 PLC
    bool sendResult = m_pPLCModbusClient->SendCoordinateToPLC(coord);
    if (!sendResult) {
        LOG_ERROR("Failed to send coordinate data to PLC\n");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            pStatus->OnStatusUpdate("坐标数据发送失败");
        }
        // 恢复轮询
        m_pPLCModbusClient->ResumePhotoRequestPolling();
        return;
    }

    // 通知 PLC 数据输出完成
    bool notifyResult = m_pPLCModbusClient->NotifyDataComplete();
    if (!notifyResult) {
        LOG_ERROR("Failed to notify PLC data complete\n");
        if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
            pStatus->OnStatusUpdate("数据完成通知失败");
        }
        // 恢复轮询
        m_pPLCModbusClient->ResumePhotoRequestPolling();
        return;
    }

    LOG_INFO("Workpiece position sent successfully and PLC notified\n");
    if (auto pStatus = GetStatusCallback<IYWorkpieceHoleStatus>()) {
        pStatus->OnStatusUpdate("已发送工件坐标到PLC");
    }

    // 发送完成，恢复拍照请求轮询
    m_pPLCModbusClient->ResumePhotoRequestPolling();
}