GrabBag/App/HoleDetection/HoleDetectionApp/dialogalgoarg.cpp

#include "dialogalgoarg.h"
#include "ui_dialogalgoarg.h"
#include <QMessageBox>
#include <QPushButton>
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include <cstring>
#include <QSettings>
#include <QFileDialog>
#include "PathManager.h"
#include "StyledMessageBox.h"

#include "VrLog.h"

DialogAlgoarg::DialogAlgoarg(ConfigManager* configManager, QWidget *parent)
    : QDialog(parent)
    , ui(new Ui::DialogAlgoarg)
    , m_pConfigManager(configManager)
{
    try {
        ui->setupUi(this);

        // 获取配置文件路径
        m_configFilePath = PathManager::GetInstance().GetConfigFilePath();

        // 检查配置文件路径是否有效
        if (m_configFilePath.isEmpty()) {
            StyledMessageBox::critical(this, "错误", "无法获取配置文件路径！");
            return;
        }

        // 初始化旋转顺序下拉框
        InitEulerOrderComboBox();

        // 初始化标定相机选择下拉框
        InitCalibCameraComboBox();

        // 初始化姿态输出顺序下拉框
        InitPoseOutputOrderComboBox();

        // 初始化方向向量反向下拉框
        InitDirVectorInvertComboBox();

        // 初始化排序模式下拉框
        InitSortModeComboBox();

        // 从配置文件加载数据到界面
        LoadConfigToUI();

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

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

void DialogAlgoarg::LoadConfigToUI()
{
    if (!m_pConfigManager) {
        StyledMessageBox::critical(this, "错误", "配置对象未初始化！");
        return;
    }

    try {
        // 从ConfigManager获取配置数据
        ConfigResult configData = m_pConfigManager->GetConfigResult();

        // 检查配置文件路径是否有效
        if (m_configFilePath.isEmpty()) {
            LOG_WARNING("Config file path is empty\n");
        }

        // 加载算法参数到UI
        const VrAlgorithmParams& algoParams = configData.algorithmParams;

        // 加载各个参数组
        LoadDetectionParamToUI(algoParams.detectionParam);
        LoadFilterParamToUI(algoParams.filterParam);
        LoadSortModeToUI(algoParams.sortMode);

        // 加载网络配置（PLC和机械臂服务端）
        LoadPlcRobotServerConfigToUI(configData.plcRobotServerConfig);

        // 加载手眼标定矩阵
        LoadCalibMatrixToUI();

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

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

        // 发生未知异常时使用默认参数
        ConfigResult configData;
        const VrAlgorithmParams& algoParams = configData.algorithmParams;
        LoadDetectionParamToUI(algoParams.detectionParam);
        LoadFilterParamToUI(algoParams.filterParam);
        LoadSortModeToUI(algoParams.sortMode);
    }
}

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

    ui->lineEdit_neighborCount->setText(QString::number(param.neighborCount));
    ui->lineEdit_angleThresholdPos->setText(QString::number(param.angleThresholdPos));
    ui->lineEdit_angleThresholdNeg->setText(QString::number(param.angleThresholdNeg));
    ui->lineEdit_minPitDepth->setText(QString::number(param.minPitDepth));
    ui->lineEdit_angleStep->setText(QString::number(param.angleStep));
    ui->lineEdit_maxScanRadius->setText(QString::number(param.maxScanRadius));
    ui->lineEdit_clusterEps->setText(QString::number(param.clusterEps));
    ui->lineEdit_clusterMinPoints->setText(QString::number(param.clusterMinPoints));
    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_validZThreshold->setText(QString::number(param.validZThreshold));
    ui->lineEdit_minVTransitionPoints->setText(QString::number(param.minVTransitionPoints));
    ui->lineEdit_cornerScale->setText(QString::number(param.cornerScale));
    ui->lineEdit_cornerAngleThreshold->setText(QString::number(param.cornerAngleThreshold));
    ui->lineEdit_jumpCornerTh_1->setText(QString::number(param.jumpCornerTh_1));
    ui->lineEdit_jumpCornerTh_2->setText(QString::number(param.jumpCornerTh_2));
    ui->lineEdit_minEndingGap->setText(QString::number(param.minEndingGap));
    ui->lineEdit_minEndingGap_z->setText(QString::number(param.minEndingGap_z));
}

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

    ui->lineEdit_minHoleRadius->setText(QString::number(param.minHoleRadius));
    ui->lineEdit_maxHoleRadius->setText(QString::number(param.maxHoleRadius));
    ui->lineEdit_maxEccentricity->setText(QString::number(param.maxEccentricity));
    ui->lineEdit_maxCornerRatio->setText(QString::number(param.maxCornerRatio));
    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));
}

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

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

bool DialogAlgoarg::SaveConfigFromUI()
{
    if (!m_pConfigManager) {
        return false;
    }

    try {
        // 获取当前配置
        SystemConfig systemConfig = m_pConfigManager->GetConfig();
        VrAlgorithmParams& algoParams = systemConfig.configResult.algorithmParams;

        // 保存检测参数
        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;
        }

        // 保存网络配置（PLC和机械臂服务端）
        if (!SavePlcRobotServerConfigFromUI(systemConfig.configResult.plcRobotServerConfig)) {
            StyledMessageBox::warning(this, "错误", "网络配置输入有误，请检查！");
            return false;
        }

        // 保存手眼标定矩阵到配置（多相机）
        if (!SaveCalibMatrixToConfig(systemConfig.configResult.handEyeCalibMatrixList)) {
            StyledMessageBox::warning(this, "错误", "手眼标定矩阵输入有误，请检查！");
            return false;
        }

        // 更新并保存配置到文件
        if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
            return false;
        }

        return m_pConfigManager->SaveConfigToFile(m_configFilePath.toStdString());

    } catch (const std::exception& e) {
        return false;
    }
}

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

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

    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.minPitDepth = ui->lineEdit_minPitDepth->text().toDouble(&ok);
    if (!ok) return false;

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

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

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

    param.clusterMinPoints = ui->lineEdit_clusterMinPoints->text().toInt(&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.validZThreshold = ui->lineEdit_validZThreshold->text().toDouble(&ok);
    if (!ok) return false;

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

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

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

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

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

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

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

    return true;
}

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

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

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

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

    param.maxCornerRatio = ui->lineEdit_maxCornerRatio->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;

    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::on_btn_loadCalibMatrix_clicked()
{
    QString filePath = QFileDialog::getOpenFileName(
        this,
        "选择手眼标定文件",
        QString(),
        "INI文件 (*.ini);;所有文件 (*.*)"
    );

    if (filePath.isEmpty()) {
        return;
    }

    LoadCalibMatrixFromFile(filePath);
    StyledMessageBox::information(this, "提示",
        QString("已从文件导入标定矩阵到相机 %1").arg(m_currentCalibCameraIndex + 1));
}

void DialogAlgoarg::onCalibCameraChanged(int index)
{
    // 切换前，先保存当前相机的矩阵到缓存
    SaveCurrentCalibMatrixToCache();

    // 更新当前选中的相机索引
    m_currentCalibCameraIndex = index;

    // 如果缓存中有对应相机的数据，加载到 UI
    LoadCalibMatrixForCamera(index);
}

void DialogAlgoarg::InitCalibCameraComboBox()
{
    if (!m_pConfigManager) return;

    ConfigResult configData = m_pConfigManager->GetConfigResult();

    // 初始化标定矩阵缓存
    m_calibMatrixCache = configData.handEyeCalibMatrixList;

    // 根据相机列表填充下拉框
    ui->comboBox_calibCamera->blockSignals(true);
    ui->comboBox_calibCamera->clear();

    if (configData.cameraList.empty()) {
        // 没有相机配置时，至少添加一个默认项
        ui->comboBox_calibCamera->addItem("相机 1", 0);
        // 确保缓存至少有一个元素
        if (m_calibMatrixCache.empty()) {
            m_calibMatrixCache.push_back(VrHandEyeCalibMatrix());
        }
    } else {
        for (size_t i = 0; i < configData.cameraList.size(); i++) {
            QString cameraName = QString::fromStdString(configData.cameraList[i].name);
            if (cameraName.isEmpty()) {
                cameraName = QString("相机 %1").arg(i + 1);
            }
            ui->comboBox_calibCamera->addItem(cameraName, static_cast<int>(i));
        }
        // 确保缓存数量与相机数量匹配
        while (m_calibMatrixCache.size() < configData.cameraList.size()) {
            m_calibMatrixCache.push_back(VrHandEyeCalibMatrix());
        }
    }

    ui->comboBox_calibCamera->blockSignals(false);

    // 连接信号
    connect(ui->comboBox_calibCamera, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &DialogAlgoarg::onCalibCameraChanged);

    m_currentCalibCameraIndex = 0;
}

QLineEdit* DialogAlgoarg::GetCalibLineEdit(int row, int col)
{
    // 通过名称查找对应的QLineEdit
    QString name = QString("lineEdit_calib_%1_%2").arg(row).arg(col);
    return findChild<QLineEdit*>(name);
}

void DialogAlgoarg::LoadCalibMatrixToUI()
{
    // 加载当前选中相机的标定矩阵
    LoadCalibMatrixForCamera(m_currentCalibCameraIndex);
}

void DialogAlgoarg::LoadCalibMatrixForCamera(int cameraIndex)
{
    if (cameraIndex < 0 || cameraIndex >= static_cast<int>(m_calibMatrixCache.size())) {
        // 索引无效，显示单位矩阵
        VrHandEyeCalibMatrix defaultMatrix;
        for (int i = 0; i < 16; i++) {
            QLineEdit* edit = GetCalibLineEdit(i / 4, i % 4);
            if (edit) {
                edit->setText(QString::number(defaultMatrix.matrix[i], 'f', 6));
            }
        }
        // 默认旋转顺序
        int index = ui->comboBox_eulerOrder->findData(11);
        if (index >= 0) ui->comboBox_eulerOrder->setCurrentIndex(index);
        return;
    }

    const VrHandEyeCalibMatrix& calibMatrix = m_calibMatrixCache[cameraIndex];

    for (int i = 0; i < 16; i++) {
        QLineEdit* edit = GetCalibLineEdit(i / 4, i % 4);
        if (edit) {
            edit->setText(QString::number(calibMatrix.matrix[i], 'f', 6));
        }
    }

    // 加载旋转顺序
    int eulerOrder = calibMatrix.eulerOrder;
    int index = ui->comboBox_eulerOrder->findData(eulerOrder);
    if (index >= 0) {
        ui->comboBox_eulerOrder->setCurrentIndex(index);
    }
}

void DialogAlgoarg::LoadCalibMatrixFromFile(const QString& filePath)
{
    QSettings settings(filePath, QSettings::IniFormat);

    // 根据当前选中的相机索引决定 INI section
    QString section = QString("CalibMatrixInfo_%1").arg(m_currentCalibCameraIndex);
    settings.beginGroup(section);

    // 如果该 section 不存在，尝试默认 section
    if (settings.childKeys().isEmpty()) {
        settings.endGroup();
        settings.beginGroup("CalibMatrixInfo_0");
    }

    for (int i = 0; i < 16; i++) {
        int row = i / 4;
        int col = i % 4;
        QString key = QString("dCalibMatrix_%1").arg(i);
        double val = settings.value(key, (row == col) ? 1.0 : 0.0).toDouble();

        QLineEdit* edit = GetCalibLineEdit(row, col);
        if (edit) {
            edit->setText(QString::number(val, 'f', 6));
        }
    }

    settings.endGroup();
}

bool DialogAlgoarg::SaveCurrentCalibMatrixToCache()
{
    if (m_currentCalibCameraIndex < 0 || m_currentCalibCameraIndex >= static_cast<int>(m_calibMatrixCache.size())) {
        return false;
    }

    VrHandEyeCalibMatrix& calibMatrix = m_calibMatrixCache[m_currentCalibCameraIndex];

    for (int i = 0; i < 16; i++) {
        QLineEdit* edit = GetCalibLineEdit(i / 4, i % 4);
        if (!edit) return false;

        bool ok = false;
        calibMatrix.matrix[i] = edit->text().toDouble(&ok);
        if (!ok) return false;
    }
    calibMatrix.eulerOrder = ui->comboBox_eulerOrder->currentData().toInt();
    return true;
}

bool DialogAlgoarg::SaveCalibMatrixToConfig(std::vector<VrHandEyeCalibMatrix>& calibMatrixList)
{
    // 先保存当前编辑中的相机矩阵到缓存
    if (!SaveCurrentCalibMatrixToCache()) {
        return false;
    }

    // 将缓存写入输出参数
    calibMatrixList = m_calibMatrixCache;
    return true;
}

void DialogAlgoarg::InitEulerOrderComboBox()
{
    // 添加外旋旋转顺序选项（工业机械臂常用）
    ui->comboBox_eulerOrder->addItem("RZ-RY-RX (外旋ZYX)", 11);  // ABB、KUKA、发那科等常用
    ui->comboBox_eulerOrder->addItem("RX-RY-RZ (外旋XYZ)", 10);
    ui->comboBox_eulerOrder->addItem("RZ-RX-RY (外旋ZXY)", 12);
    ui->comboBox_eulerOrder->addItem("RY-RX-RZ (外旋YXZ)", 13);
    ui->comboBox_eulerOrder->addItem("RY-RZ-RX (外旋YZX)", 14);
    ui->comboBox_eulerOrder->addItem("RX-RZ-RY (外旋XZY)", 15);

    // 默认选择外旋 ZYX
    ui->comboBox_eulerOrder->setCurrentIndex(0);
}

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

    // 默认选择 RPY
    ui->comboBox_poseOutputOrder->setCurrentIndex(0);
}

void DialogAlgoarg::InitDirVectorInvertComboBox()
{
    // 添加方向向量反向选项
    ui->comboBox_dirVectorInvert->addItem("不反向", DIR_INVERT_NONE);
    ui->comboBox_dirVectorInvert->addItem("XY反向", DIR_INVERT_XY);
    ui->comboBox_dirVectorInvert->addItem("XZ反向", DIR_INVERT_XZ);
    ui->comboBox_dirVectorInvert->addItem("YZ反向（默认）", DIR_INVERT_YZ);

    // 默认选择 YZ反向（兼容原有行为）
    ui->comboBox_dirVectorInvert->setCurrentIndex(3);
}

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

void DialogAlgoarg::LoadPlcRobotServerConfigToUI(const VrPlcRobotServerConfig& config)
{
    if (!ui) return;

    // 加载TCP协议端口
    ui->lineEdit_tcpServerPort->setText(QString::number(config.tcpServerPort));

    // 加载姿态输出顺序
    int poseOrderIndex = ui->comboBox_poseOutputOrder->findData(config.poseOutputOrder);
    if (poseOrderIndex >= 0) {
        ui->comboBox_poseOutputOrder->setCurrentIndex(poseOrderIndex);
    }

    // 加载方向向量反向配置
    int dirInvertIndex = ui->comboBox_dirVectorInvert->findData(config.dirVectorInvert);
    if (dirInvertIndex >= 0) {
        ui->comboBox_dirVectorInvert->setCurrentIndex(dirInvertIndex);
    }
}

bool DialogAlgoarg::SavePlcRobotServerConfigFromUI(VrPlcRobotServerConfig& config)
{
    if (!ui) return false;

    // 获取TCP协议端口
    bool ok = false;
    config.tcpServerPort = ui->lineEdit_tcpServerPort->text().toInt(&ok);
    if (!ok || config.tcpServerPort <= 0 || config.tcpServerPort > 65535) {
        return false;
    }

    // 获取姿态输出顺序
    config.poseOutputOrder = ui->comboBox_poseOutputOrder->currentData().toInt();

    // 获取方向向量反向配置
    config.dirVectorInvert = ui->comboBox_dirVectorInvert->currentData().toInt();

    return true;
}