GrabBag/App/HoleDetection/HoleDetectionApp/dialogalgoarg.cpp

#include "dialogalgoarg.h"
#include "ui_dialogalgoarg.h"

#include "HoleDetectionPresenter.h"
#include "HandEyeCalibWidget.h"
#include "NetworkConfigWidget.h"
#include "PathManager.h"
#include "StyledMessageBox.h"
#include "VrLog.h"

#include <algorithm>
#include <cstring>

DialogAlgoarg::DialogAlgoarg(ConfigManager* configManager, HoleDetectionPresenter* presenter, QWidget *parent)
    : QDialog(parent)
    , ui(new Ui::DialogAlgoarg)
    , m_pConfigManager(configManager)
    , m_presenter(presenter)
    , m_handEyeCalibWidget(nullptr)
    , m_networkConfigWidget(nullptr)
{
    try {
        ui->setupUi(this);

        m_configFilePath = PathManager::GetInstance().GetConfigFilePath();
        if (m_configFilePath.isEmpty()) {
            StyledMessageBox::critical(this, "错误", "无法获取配置文件路径！");
            return;
        }

        InitSortModeComboBox();
        InitPoseOutputOrderComboBox();
        InitHandEyeCalibTab();
        InitNetworkConfigTab();
        LoadConfigToUI();
    } catch (const std::exception& e) {
        StyledMessageBox::critical(this, "初始化错误", QString("对话框初始化失败: %1").arg(e.what()));
    } catch (...) {
        StyledMessageBox::critical(this, "初始化错误", "对话框初始化失败！（未知错误）");
    }
}

DialogAlgoarg::~DialogAlgoarg()
{
    delete ui;
}

// 手眼标定页复用共享控件，这里根据 Presenter 当前相机列表初始化并回填已有矩阵。
void DialogAlgoarg::InitHandEyeCalibTab()
{
    if (!m_presenter) {
        return;
    }

    m_handEyeCalibWidget = new HandEyeCalibWidget(this);
    m_handEyeCalibWidget->setMatrixEditable(true);
    ui->verticalLayout_handEyeCalibHost->addWidget(m_handEyeCalibWidget);

    m_handEyeCalibWidget->setDefaultFilePath(PathManager::GetInstance().GetAppConfigDirectory());

    const auto& cameraList = m_presenter->GetCameraList();

    QVector<HandEyeCalibCameraInfo> calibCameraList;
    if (cameraList.empty()) {
        HandEyeCalibCameraInfo defaultCam;
        defaultCam.cameraIndex = 1;
        defaultCam.displayName = QString::fromUtf8("相机 1");
        calibCameraList.append(defaultCam);
    } else {
        for (size_t i = 0; i < cameraList.size(); ++i) {
            HandEyeCalibCameraInfo info;
            info.cameraIndex = static_cast<int>(i + 1);
            info.displayName = QString::fromStdString(cameraList[i].first);
            calibCameraList.append(info);
        }
    }
    m_handEyeCalibWidget->setCameraList(calibCameraList);

    for (const auto& camInfo : calibCameraList) {
        CalibMatrix calibMatrix = m_presenter->GetClibMatrix(camInfo.cameraIndex - 1);
        bool hasCalibData = false;
        for (int i = 0; i < 16; ++i) {
            const double identity = (i / 4 == i % 4) ? 1.0 : 0.0;
            if (qAbs(calibMatrix.clibMatrix[i] - identity) > 1e-9) {
                hasCalibData = true;
                break;
            }
        }
        m_handEyeCalibWidget->setCalibData(camInfo.cameraIndex, calibMatrix.clibMatrix, hasCalibData);
    }

    connect(m_handEyeCalibWidget, &HandEyeCalibWidget::calibMatrixLoaded,
            this, &DialogAlgoarg::onCalibMatrixLoaded);
    connect(m_handEyeCalibWidget, &HandEyeCalibWidget::saveCalibRequested,
            this, &DialogAlgoarg::onSaveCalibRequested);
}

void DialogAlgoarg::onCalibMatrixLoaded(int cameraIndex, const double* matrix)
{
    Q_UNUSED(cameraIndex);
    Q_UNUSED(matrix);
    StyledMessageBox::information(this, "提示", "已从文件导入标定矩阵");
}

void DialogAlgoarg::onSaveCalibRequested(int cameraIndex, const double* matrix)
{
    Q_UNUSED(cameraIndex);
    Q_UNUSED(matrix);

    if (SaveConfigFromUI()) {
        StyledMessageBox::information(this, "成功", "手眼标定参数已保存！");
    } else {
        StyledMessageBox::warning(this, "失败", "保存手眼标定参数失败！");
    }
}

// 当前对话框只暴露 TCP 相关网络配置，不包含其它应用里的 PLC 细项。
void DialogAlgoarg::InitNetworkConfigTab()
{
    if (!m_pConfigManager) {
        return;
    }

    m_networkConfigWidget = new NetworkConfigWidget(false, true, this);
    ui->verticalLayout_networkConfigHost->addWidget(m_networkConfigWidget);
}

void DialogAlgoarg::LoadNetworkConfigToUI()
{
    if (!m_pConfigManager || !m_networkConfigWidget) {
        return;
    }

    ConfigResult configResult = m_pConfigManager->GetConfigResult();
    const VrPlcRobotServerConfig& plcConfig = configResult.plcRobotServerConfig;

    NetworkConfigData netConfig;
    netConfig.tcpServerPort = plcConfig.tcpServerPort;
    netConfig.dirVectorInvert = plcConfig.dirVectorInvert;

    if (!configResult.handEyeCalibMatrixList.empty()) {
        netConfig.eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder;
    }

    m_networkConfigWidget->setConfig(netConfig);

    const int poseOrderIndex = ui->comboBox_poseOutputOrder->findData(plcConfig.poseOutputOrder);
    if (poseOrderIndex >= 0) {
        ui->comboBox_poseOutputOrder->setCurrentIndex(poseOrderIndex);
    }
}

// 从当前配置结果加载整页参数；如果读取异常，则退回默认构造参数显示。
void DialogAlgoarg::LoadConfigToUI()
{
    if (!m_pConfigManager) {
        StyledMessageBox::critical(this, "错误", "配置对象未初始化！");
        return;
    }

    try {
        const ConfigResult configData = m_pConfigManager->GetConfigResult();
        const VrAlgorithmParams& algoParams = configData.algorithmParams;

        LoadRansacParamToUI(algoParams.ransacParam);
        LoadDetectionParamToUI(algoParams.detectionParam);
        LoadFilterParamToUI(algoParams.filterParam);
        LoadSortModeToUI(algoParams.sortMode);
        LoadNetworkConfigToUI();
    } catch (const std::exception& e) {
        LOG_ERROR("Exception in LoadConfigToUI: %s\n", e.what());
        StyledMessageBox::warning(this, "警告",
            QString("加载配置时发生异常: %1\n将使用默认参数显示").arg(e.what()));

        const ConfigResult configData;
        const VrAlgorithmParams& algoParams = configData.algorithmParams;
        LoadRansacParamToUI(algoParams.ransacParam);
        LoadDetectionParamToUI(algoParams.detectionParam);
        LoadFilterParamToUI(algoParams.filterParam);
        LoadSortModeToUI(algoParams.sortMode);
    } catch (...) {
        LOG_ERROR("Unknown exception in LoadConfigToUI\n");
        StyledMessageBox::warning(this, "警告", "加载配置文件失败（未知错误），将使用默认参数显示");

        const ConfigResult configData;
        const VrAlgorithmParams& algoParams = configData.algorithmParams;
        LoadRansacParamToUI(algoParams.ransacParam);
        LoadDetectionParamToUI(algoParams.detectionParam);
        LoadFilterParamToUI(algoParams.filterParam);
        LoadSortModeToUI(algoParams.sortMode);
    }
}

// 从界面采集参数，合并回完整 SystemConfig，再整体落盘，避免只保存局部配置。
bool DialogAlgoarg::SaveConfigFromUI()
{
    if (!m_pConfigManager) {
        return false;
    }

    try {
        SystemConfig systemConfig = m_pConfigManager->GetConfig();
        VrAlgorithmParams& algoParams = systemConfig.configResult.algorithmParams;

        if (!SaveRansacParamFromUI(algoParams.ransacParam)) {
            StyledMessageBox::warning(this, "错误", "RANSAC 参数输入有误，请检查！");
            return false;
        }

        if (!SaveDetectionParamFromUI(algoParams.detectionParam)) {
            StyledMessageBox::warning(this, "错误", "检测参数输入有误，请检查！");
            return false;
        }

        if (!SaveFilterParamFromUI(algoParams.filterParam)) {
            StyledMessageBox::warning(this, "错误", "过滤参数输入有误，请检查！");
            return false;
        }

        if (!SaveSortModeFromUI(algoParams.sortMode)) {
            StyledMessageBox::warning(this, "错误", "排序模式设置有误，请检查！");
            return false;
        }

        if (m_handEyeCalibWidget && m_presenter) {
            const auto& oldMatrixList = systemConfig.configResult.handEyeCalibMatrixList;
            std::vector<VrHandEyeCalibMatrix> newMatrixList;

            const auto& cameraList = m_presenter->GetCameraList();
            const int cameraCount = std::max(1, static_cast<int>(cameraList.size()));

            for (int i = 0; i < cameraCount; ++i) {
                const int camIdx = i + 1;

                // 先以已有配置为基准，避免未编辑的相机矩阵被默认值覆盖。
                VrHandEyeCalibMatrix calibMatrix;
                calibMatrix.cameraIndex = camIdx;
                for (const auto& old : oldMatrixList) {
                    if (old.cameraIndex == camIdx) {
                        calibMatrix = old;
                        break;
                    }
                }

                bool isCalibrated = false;
                double matrix[16];
                // 仅当控件里存在有效标定矩阵时才覆盖，未标定相机保留原值。
                if (m_handEyeCalibWidget->getCalibData(camIdx, matrix, isCalibrated) && isCalibrated) {
                    memcpy(calibMatrix.matrix, matrix, sizeof(double) * 16);
                }

                newMatrixList.push_back(calibMatrix);
            }
            systemConfig.configResult.handEyeCalibMatrixList = newMatrixList;
        }

        if (m_networkConfigWidget) {
            const NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
            systemConfig.configResult.plcRobotServerConfig.tcpServerPort = netConfig.tcpServerPort;
            systemConfig.configResult.plcRobotServerConfig.dirVectorInvert = netConfig.dirVectorInvert;

            // 欧拉角顺序是和姿态输出一起配置的，这里同步回所有手眼矩阵记录。
            for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
                calibMatrix.eulerOrder = netConfig.eulerOrder;
            }
        }

        systemConfig.configResult.plcRobotServerConfig.poseOutputOrder =
            ui->comboBox_poseOutputOrder->currentData().toInt();

        if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
            return false;
        }

        if (!m_pConfigManager->SaveConfigToFile(m_configFilePath.toStdString())) {
            return false;
        }

        if (m_presenter) {
            m_presenter->OnConfigChanged(systemConfig.configResult);
        }

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

// 各参数组分别做 UI 映射，方便后续跟算法头文件逐项对照。
void DialogAlgoarg::LoadRansacParamToUI(const VrRansacPlaneSegmentationParam& param)
{
    if (!ui) {
        return;
    }

    ui->lineEdit_distanceThreshold->setText(QString::number(param.distanceThreshold));
    ui->lineEdit_maxIterations->setText(QString::number(param.maxIterations));
    ui->lineEdit_minPlanePoints->setText(QString::number(param.minPlanePoints));
    ui->lineEdit_maxPlanes->setText(QString::number(param.maxPlanes));
    ui->lineEdit_growthZThreshold->setText(QString::number(param.growthZThreshold));
    ui->lineEdit_minPlaneRatio->setText(QString::number(param.minPlaneRatio));
    ui->lineEdit_maxNormalAngleDeg->setText(QString::number(param.maxNormalAngleDeg));
    ui->lineEdit_maxDistFromPlane->setText(QString::number(param.maxDistFromPlane));
}

void DialogAlgoarg::LoadDetectionParamToUI(const VrHoleDetectionParam& param)
{
    if (!ui) {
        return;
    }

    ui->lineEdit_angleThresholdPos->setText(QString::number(param.angleThresholdPos));
    ui->lineEdit_angleThresholdNeg->setText(QString::number(param.angleThresholdNeg));
    ui->lineEdit_angleSearchDistance->setText(QString::number(param.angleSearchDistance));
    ui->lineEdit_minPitDepth->setText(QString::number(param.minPitDepth));
    ui->lineEdit_minRadius->setText(QString::number(param.minRadius));
    ui->lineEdit_maxRadius->setText(QString::number(param.maxRadius));
    ui->lineEdit_expansionSize1->setText(QString::number(param.expansionSize1));
    ui->lineEdit_expansionSize2->setText(QString::number(param.expansionSize2));
    ui->lineEdit_minVTransitionPoints->setText(QString::number(param.minVTransitionPoints));
    ui->lineEdit_jumpThresholdResidual->setText(QString::number(param.jumpThresholdResidual));
    ui->lineEdit_gapJumpThresholdResidual->setText(QString::number(param.gapJumpThresholdResidual));
    ui->lineEdit_maxGapPointsInLine->setText(QString::number(param.maxGapPointsInLine));
    ui->lineEdit_minFeatureSpan->setText(QString::number(param.minFeatureSpan));
    ui->lineEdit_residualSmoothWindow->setText(QString::number(param.residualSmoothWindow));
    ui->lineEdit_slopeAngleThreshold->setText(QString::number(param.slopeAngleThreshold));
    ui->lineEdit_edgeBoundaryFilterDist->setText(QString::number(param.edgeBoundaryFilterDist));
}

void DialogAlgoarg::LoadFilterParamToUI(const VrHoleFilterParam& param)
{
    if (!ui) {
        return;
    }

    ui->lineEdit_maxEccentricity->setText(QString::number(param.maxEccentricity));
    ui->lineEdit_minAngularCoverage->setText(QString::number(param.minAngularCoverage));
    ui->lineEdit_maxRadiusFitRatio->setText(QString::number(param.maxRadiusFitRatio));
    ui->lineEdit_minQualityScore->setText(QString::number(param.minQualityScore));
    ui->lineEdit_maxPlaneResidual->setText(QString::number(param.maxPlaneResidual));
    ui->lineEdit_maxAngularGap->setText(QString::number(param.maxAngularGap));
    ui->lineEdit_minInlierRatio->setText(QString::number(param.minInlierRatio));
    ui->lineEdit_minHoleDepth->setText(QString::number(param.minHoleDepth));
}

void DialogAlgoarg::LoadSortModeToUI(int sortMode)
{
    if (!ui) {
        return;
    }

    const int index = ui->comboBox_sortMode->findData(sortMode);
    if (index >= 0) {
        ui->comboBox_sortMode->setCurrentIndex(index);
    }
}

// 保存时逐项做文本转数值校验，避免无效输入直接污染配置对象。
bool DialogAlgoarg::SaveRansacParamFromUI(VrRansacPlaneSegmentationParam& param)
{
    bool ok = true;

    param.distanceThreshold = ui->lineEdit_distanceThreshold->text().toDouble(&ok);
    if (!ok) return false;

    param.maxIterations = ui->lineEdit_maxIterations->text().toInt(&ok);
    if (!ok) return false;

    param.minPlanePoints = ui->lineEdit_minPlanePoints->text().toInt(&ok);
    if (!ok) return false;

    param.maxPlanes = ui->lineEdit_maxPlanes->text().toInt(&ok);
    if (!ok) return false;

    param.growthZThreshold = ui->lineEdit_growthZThreshold->text().toDouble(&ok);
    if (!ok) return false;

    param.minPlaneRatio = ui->lineEdit_minPlaneRatio->text().toDouble(&ok);
    if (!ok) return false;

    param.maxNormalAngleDeg = ui->lineEdit_maxNormalAngleDeg->text().toDouble(&ok);
    if (!ok) return false;

    param.maxDistFromPlane = ui->lineEdit_maxDistFromPlane->text().toDouble(&ok);
    if (!ok) return false;

    return true;
}

bool DialogAlgoarg::SaveDetectionParamFromUI(VrHoleDetectionParam& param)
{
    bool ok = true;

    param.angleThresholdPos = ui->lineEdit_angleThresholdPos->text().toDouble(&ok);
    if (!ok) return false;

    param.angleThresholdNeg = ui->lineEdit_angleThresholdNeg->text().toDouble(&ok);
    if (!ok) return false;

    param.angleSearchDistance = ui->lineEdit_angleSearchDistance->text().toDouble(&ok);
    if (!ok) return false;

    param.minPitDepth = ui->lineEdit_minPitDepth->text().toDouble(&ok);
    if (!ok) return false;

    param.minRadius = ui->lineEdit_minRadius->text().toDouble(&ok);
    if (!ok) return false;

    param.maxRadius = ui->lineEdit_maxRadius->text().toDouble(&ok);
    if (!ok) return false;

    param.expansionSize1 = ui->lineEdit_expansionSize1->text().toInt(&ok);
    if (!ok) return false;

    param.expansionSize2 = ui->lineEdit_expansionSize2->text().toInt(&ok);
    if (!ok) return false;

    param.minVTransitionPoints = ui->lineEdit_minVTransitionPoints->text().toInt(&ok);
    if (!ok) return false;

    param.jumpThresholdResidual = ui->lineEdit_jumpThresholdResidual->text().toDouble(&ok);
    if (!ok) return false;

    param.gapJumpThresholdResidual = ui->lineEdit_gapJumpThresholdResidual->text().toDouble(&ok);
    if (!ok) return false;

    param.maxGapPointsInLine = ui->lineEdit_maxGapPointsInLine->text().toInt(&ok);
    if (!ok) return false;

    param.minFeatureSpan = ui->lineEdit_minFeatureSpan->text().toDouble(&ok);
    if (!ok) return false;

    param.residualSmoothWindow = ui->lineEdit_residualSmoothWindow->text().toInt(&ok);
    if (!ok) return false;

    param.slopeAngleThreshold = ui->lineEdit_slopeAngleThreshold->text().toDouble(&ok);
    if (!ok) return false;

    param.edgeBoundaryFilterDist = ui->lineEdit_edgeBoundaryFilterDist->text().toDouble(&ok);
    if (!ok) return false;

    return true;
}

bool DialogAlgoarg::SaveFilterParamFromUI(VrHoleFilterParam& param)
{
    bool ok = true;

    param.maxEccentricity = ui->lineEdit_maxEccentricity->text().toDouble(&ok);
    if (!ok) return false;

    param.minAngularCoverage = ui->lineEdit_minAngularCoverage->text().toDouble(&ok);
    if (!ok) return false;

    param.maxRadiusFitRatio = ui->lineEdit_maxRadiusFitRatio->text().toDouble(&ok);
    if (!ok) return false;

    param.minQualityScore = ui->lineEdit_minQualityScore->text().toDouble(&ok);
    if (!ok) return false;

    param.maxPlaneResidual = ui->lineEdit_maxPlaneResidual->text().toDouble(&ok);
    if (!ok) return false;

    param.maxAngularGap = ui->lineEdit_maxAngularGap->text().toDouble(&ok);
    if (!ok) return false;

    param.minInlierRatio = ui->lineEdit_minInlierRatio->text().toDouble(&ok);
    if (!ok) return false;

    param.minHoleDepth = ui->lineEdit_minHoleDepth->text().toDouble(&ok);
    if (!ok) return false;

    return true;
}

bool DialogAlgoarg::SaveSortModeFromUI(int& sortMode)
{
    if (!ui) {
        return false;
    }
    sortMode = ui->comboBox_sortMode->currentData().toInt();
    return true;
}

// 点击确定时保存整份配置，而不是只保存当前页签里的算法参数。
void DialogAlgoarg::on_btn_camer_ok_clicked()
{
    if (SaveConfigFromUI()) {
        StyledMessageBox::information(this, "成功", "配置保存成功！");
        accept();
    } else {
        StyledMessageBox::warning(this, "失败", "配置保存失败，请检查文件权限或参数输入！");
    }
}

void DialogAlgoarg::on_btn_camer_cancel_clicked()
{
    reject();
}

void DialogAlgoarg::InitPoseOutputOrderComboBox()
{
    ui->comboBox_poseOutputOrder->addItem("RPY (Roll, Pitch, Yaw)", POSE_ORDER_RPY);
    ui->comboBox_poseOutputOrder->addItem("RYP (Roll, Yaw, Pitch)", POSE_ORDER_RYP);
    ui->comboBox_poseOutputOrder->addItem("PRY (Pitch, Roll, Yaw)", POSE_ORDER_PRY);
    ui->comboBox_poseOutputOrder->addItem("PYR (Pitch, Yaw, Roll)", POSE_ORDER_PYR);
    ui->comboBox_poseOutputOrder->addItem("YRP (Yaw, Roll, Pitch)", POSE_ORDER_YRP);
    ui->comboBox_poseOutputOrder->addItem("YPR (Yaw, Pitch, Roll)", POSE_ORDER_YPR);
    ui->comboBox_poseOutputOrder->setCurrentIndex(0);
}

void DialogAlgoarg::InitSortModeComboBox()
{
    ui->comboBox_sortMode->addItem("不排序", HOLE_SORT_NONE);
    ui->comboBox_sortMode->addItem("按半径排序（最大优先）", HOLE_SORT_BY_RADIUS);
    ui->comboBox_sortMode->addItem("按深度排序（最深优先）", HOLE_SORT_BY_DEPTH);
    ui->comboBox_sortMode->addItem("按质量分数排序（最高优先）", HOLE_SORT_BY_QUALITY);
    ui->comboBox_sortMode->setCurrentIndex(0);
}