GrabBag/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp

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

#include "HandEyeCalibWidget.h"
#include "NetworkConfigWidget.h"
#include "PathManager.h"
#include "ScrewPositionPresenter.h"
#include "StyledMessageBox.h"

#include <QDoubleValidator>
#include <QIntValidator>
#include <QLineEdit>

#include <algorithm>
#include <cmath>
#include <cstring>

namespace {

bool IsIdentityMatrix(const double matrix[16])
{
    for (int i = 0; i < 16; ++i) {
        const double expected = (i / 4 == i % 4) ? 1.0 : 0.0;
        if (std::fabs(matrix[i] - expected) > 1e-9) {
            return false;
        }
    }
    return true;
}

}

DialogAlgoArg::DialogAlgoArg(QWidget *parent)
    : QDialog(parent)
    , ui(new Ui::DialogAlgoArg)
{
    ui->setupUi(this);
    setWindowTitle(QStringLiteral("算法参数设置"));

    initNumericEditors();
    initHandEyeCalibTab();
    initNetworkConfigTab();
}

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

void DialogAlgoArg::SetPresenter(ScrewPositionPresenter* presenter)
{
    m_presenter = presenter;
    loadParams();
    loadHandEyeCalibConfig();
    loadNetworkConfig();
}

void DialogAlgoArg::initNumericEditors()
{
    auto setupDoubleEditor = [](QLineEdit* edit, double min, double max, int decimals) {
        auto* validator = new QDoubleValidator(min, max, decimals, edit);
        validator->setNotation(QDoubleValidator::StandardNotation);
        edit->setValidator(validator);
    };

    auto setupIntEditor = [](QLineEdit* edit, int min, int max) {
        edit->setValidator(new QIntValidator(min, max, edit));
    };

    setupDoubleEditor(ui->spinRodDiameter, 1.0, 100.0, 6);
    setupDoubleEditor(ui->spinCornerTh, 0.0, 180.0, 6);
    setupDoubleEditor(ui->spinScale, 0.0, 200.0, 6);
    setupDoubleEditor(ui->spinMinEndingGap, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinMinEndingGapZ, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinJumpCornerTh1, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinJumpCornerTh2, 0.0, 180.0, 6);
    setupDoubleEditor(ui->spinContinuityTh, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinOutlierTh, 0.0, 50.0, 6);
    setupIntEditor(ui->spinMaxLineSkipNum, -1, 100);
    setupDoubleEditor(ui->spinYDeviationMax, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinMaxSkipDistance, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinZDeviationMax, 0.0, 100.0, 6);
    setupDoubleEditor(ui->spinMinLTypeTreeLen, 0.0, 1000.0, 6);
    setupDoubleEditor(ui->spinMinVTypeTreeLen, 0.0, 1000.0, 6);
}

void DialogAlgoArg::initHandEyeCalibTab()
{
    if (!ui || !ui->verticalLayout_handEyeCalibHost || m_handEyeCalibWidget) {
        return;
    }

    m_handEyeCalibWidget = new HandEyeCalibWidget(this);
    m_handEyeCalibWidget->setMatrixEditable(true);
    m_handEyeCalibWidget->setExtrinsicControlsVisible(true);
    m_handEyeCalibWidget->setApproachOffsetVisible(true);
    m_handEyeCalibWidget->setDefaultFilePath(PathManager::GetInstance().GetAppConfigDirectory());
    ui->verticalLayout_handEyeCalibHost->addWidget(m_handEyeCalibWidget);

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

void DialogAlgoArg::initNetworkConfigTab()
{
    if (!ui || !ui->verticalLayout_networkConfigHost || m_networkConfigWidget) {
        return;
    }

    m_networkConfigWidget = new NetworkConfigWidget(false, true, this);
    // 欧拉角顺序、方向向量调整、长边对应轴已迁移到手眼标定页
    m_networkConfigWidget->setExtrinsicControlsVisible(false);
    ui->verticalLayout_networkConfigHost->addWidget(m_networkConfigWidget);
}

void DialogAlgoArg::setDoubleEditorText(QLineEdit* edit, double value)
{
    if (edit) {
        edit->setText(QString::number(value, 'g', 12));
    }
}

void DialogAlgoArg::setIntEditorText(QLineEdit* edit, int value)
{
    if (edit) {
        edit->setText(QString::number(value));
    }
}

bool DialogAlgoArg::tryGetDouble(QLineEdit* edit, const QString& label, double& value)
{
    bool ok = false;
    value = edit->text().trimmed().toDouble(&ok);
    if (!ok) {
        StyledMessageBox::warning(this, QStringLiteral("错误"),
            QStringLiteral("“%1” 输入的数值无效。").arg(label));
        edit->setFocus();
        edit->selectAll();
        return false;
    }
    return true;
}

bool DialogAlgoArg::tryGetInt(QLineEdit* edit, const QString& label, int& value)
{
    bool ok = false;
    value = edit->text().trimmed().toInt(&ok);
    if (!ok) {
        StyledMessageBox::warning(this, QStringLiteral("错误"),
            QStringLiteral("“%1” 输入的整数无效。").arg(label));
        edit->setFocus();
        edit->selectAll();
        return false;
    }
    return true;
}

void DialogAlgoArg::loadParams()
{
    if (!m_presenter) {
        return;
    }

    const auto params = m_presenter->GetAlgoParams();

    setDoubleEditorText(ui->spinRodDiameter, params.screwParam.rodDiameter);
    ui->chkHorizonScan->setChecked(params.screwParam.isHorizonScan);

    setDoubleEditorText(ui->spinCornerTh, params.cornerParam.cornerTh);
    setDoubleEditorText(ui->spinScale, params.cornerParam.scale);
    setDoubleEditorText(ui->spinMinEndingGap, params.cornerParam.minEndingGap);
    setDoubleEditorText(ui->spinMinEndingGapZ, params.cornerParam.minEndingGap_z);
    setDoubleEditorText(ui->spinJumpCornerTh1, params.cornerParam.jumpCornerTh_1);
    setDoubleEditorText(ui->spinJumpCornerTh2, params.cornerParam.jumpCornerTh_2);

    setDoubleEditorText(ui->spinContinuityTh, params.filterParam.continuityTh);
    setDoubleEditorText(ui->spinOutlierTh, params.filterParam.outlierTh);

    setIntEditorText(ui->spinMaxLineSkipNum, params.growParam.maxLineSkipNum);
    setDoubleEditorText(ui->spinYDeviationMax, params.growParam.yDeviation_max);
    setDoubleEditorText(ui->spinMaxSkipDistance, params.growParam.maxSkipDistance);
    setDoubleEditorText(ui->spinZDeviationMax, params.growParam.zDeviation_max);
    setDoubleEditorText(ui->spinMinLTypeTreeLen, params.growParam.minLTypeTreeLen);
    setDoubleEditorText(ui->spinMinVTypeTreeLen, params.growParam.minVTypeTreeLen);
}

void DialogAlgoArg::loadHandEyeCalibConfig()
{
    if (!m_presenter || !m_handEyeCalibWidget || !m_presenter->GetConfigManager()) {
        return;
    }

    const ConfigResult configResult = m_presenter->GetConfigManager()->GetConfigResult();
    QVector<HandEyeCalibCameraInfo> cameraInfos;
    if (configResult.cameraList.empty()) {
        HandEyeCalibCameraInfo defaultCam;
        defaultCam.cameraIndex = 1;
        defaultCam.displayName = QStringLiteral("相机 1");
        cameraInfos.append(defaultCam);
    } else {
        for (size_t i = 0; i < configResult.cameraList.size(); ++i) {
            HandEyeCalibCameraInfo info;
            info.cameraIndex = static_cast<int>(i + 1);
            info.displayName = QString::fromStdString(configResult.cameraList[i].name);
            cameraInfos.append(info);
        }
    }

    m_handEyeCalibWidget->setCameraList(cameraInfos);

    for (const auto& info : cameraInfos) {
        // 以螺杆 set 作为「该相机的公共默认」展示
        const VrDetectionConfigSet* savedSet =
            configResult.FindDetectionConfig(info.cameraIndex, DETECTION_TYPE_SCREW);

        if (savedSet) {
            const auto& savedMatrix = savedSet->handEyeMatrix;
            m_handEyeCalibWidget->setCalibData(info.cameraIndex,
                                               savedMatrix.matrix,
                                               !IsIdentityMatrix(savedMatrix.matrix));
            m_handEyeCalibWidget->setExtrinsicData(info.cameraIndex,
                                                   savedMatrix.eulerOrder,
                                                   savedMatrix.rotX,
                                                   savedMatrix.rotY,
                                                   savedMatrix.rotZ,
                                                   savedMatrix.approachOffset,
                                                   savedMatrix.offsetX,
                                                   savedMatrix.offsetY,
                                                   savedMatrix.offsetZ);
        } else {
            const CalibMatrix calibMatrix = m_presenter->GetClibMatrix(info.cameraIndex - 1);
            m_handEyeCalibWidget->setCalibData(info.cameraIndex,
                                               calibMatrix.clibMatrix,
                                               !IsIdentityMatrix(calibMatrix.clibMatrix));
            m_handEyeCalibWidget->setExtrinsicData(info.cameraIndex, 11, 0.0, 0.0, 0.0, 0.0);
        }
    }
}

void DialogAlgoArg::loadNetworkConfig()
{
    if (!m_presenter || !m_presenter->GetConfigManager() || !m_networkConfigWidget) {
        return;
    }

    const ConfigResult configResult = m_presenter->GetConfigManager()->GetConfigResult();

    NetworkConfigData netConfig;
    netConfig.tcpServerPort = configResult.tcpPort;
    netConfig.poseOutputOrder = configResult.poseOutputOrder;
    netConfig.byteOrder = configResult.byteOrder;

    m_networkConfigWidget->setConfig(netConfig);
}

bool DialogAlgoArg::saveParams()
{
    if (!m_presenter || !m_presenter->GetConfigManager()) {
        return false;
    }

    SystemConfig systemConfig = m_presenter->GetConfigManager()->GetConfig();
    // 旧对话框只编辑「公共默认」语义：算法参数 + 每相机一份矩阵，
    // 都同步写到 (camera, screw) 与 (camera, toolDisk) 两个槽位上。
    VrAlgorithmParams sharedParams;
    sharedParams.planeCalibParam = systemConfig.configResult.algorithmParams.planeCalibParam;

    if (!tryGetDouble(ui->spinRodDiameter, QStringLiteral("螺杆直径"), sharedParams.screwParam.rodDiameter)) return false;
    sharedParams.screwParam.isHorizonScan = ui->chkHorizonScan->isChecked();

    if (!tryGetDouble(ui->spinCornerTh, QStringLiteral("拐角阈值"), sharedParams.cornerParam.cornerTh)) return false;
    if (!tryGetDouble(ui->spinScale, QStringLiteral("窗口比例因子"), sharedParams.cornerParam.scale)) return false;
    if (!tryGetDouble(ui->spinMinEndingGap, QStringLiteral("Y方向最小结束间隙"), sharedParams.cornerParam.minEndingGap)) return false;
    if (!tryGetDouble(ui->spinMinEndingGapZ, QStringLiteral("Z方向最小结束间隙"), sharedParams.cornerParam.minEndingGap_z)) return false;
    if (!tryGetDouble(ui->spinJumpCornerTh1, QStringLiteral("跳变拐角阈值1"), sharedParams.cornerParam.jumpCornerTh_1)) return false;
    if (!tryGetDouble(ui->spinJumpCornerTh2, QStringLiteral("跳变拐角阈值2"), sharedParams.cornerParam.jumpCornerTh_2)) return false;

    if (!tryGetDouble(ui->spinContinuityTh, QStringLiteral("连续性阈值"), sharedParams.filterParam.continuityTh)) return false;
    if (!tryGetDouble(ui->spinOutlierTh, QStringLiteral("离群点阈值"), sharedParams.filterParam.outlierTh)) return false;

    if (!tryGetInt(ui->spinMaxLineSkipNum, QStringLiteral("最大跳线数"), sharedParams.growParam.maxLineSkipNum)) return false;
    if (!tryGetDouble(ui->spinYDeviationMax, QStringLiteral("Y偏差最大值"), sharedParams.growParam.yDeviation_max)) return false;
    if (!tryGetDouble(ui->spinMaxSkipDistance, QStringLiteral("最大跳跃距离"), sharedParams.growParam.maxSkipDistance)) return false;
    if (!tryGetDouble(ui->spinZDeviationMax, QStringLiteral("Z偏差最大值"), sharedParams.growParam.zDeviation_max)) return false;
    if (!tryGetDouble(ui->spinMinLTypeTreeLen, QStringLiteral("L型树最小长度"), sharedParams.growParam.minLTypeTreeLen)) return false;
    if (!tryGetDouble(ui->spinMinVTypeTreeLen, QStringLiteral("V型树最小长度"), sharedParams.growParam.minVTypeTreeLen)) return false;

    // 把刚刚解析出来的「公共默认」算法参数写入全部 4 个 (camera, type) 槽位。
    for (auto& item : systemConfig.configResult.detectionConfigList) {
        item.algorithmParams.screwParam = sharedParams.screwParam;
        item.algorithmParams.cornerParam = sharedParams.cornerParam;
        item.algorithmParams.filterParam = sharedParams.filterParam;
        item.algorithmParams.growParam = sharedParams.growParam;
    }

    if (m_handEyeCalibWidget) {
        int cameraCount = std::max(1, static_cast<int>(systemConfig.configResult.cameraList.size()));
        // 兜底：保证 (camera N, screw) + (camera N, toolDisk) 槽位都存在
        for (int camIdx = 1; camIdx <= cameraCount; ++camIdx) {
            systemConfig.configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_SCREW);
            systemConfig.configResult.EnsureDetectionConfig(camIdx, DETECTION_TYPE_TOOL_DISK);
        }

        for (int camIdx = 1; camIdx <= cameraCount; ++camIdx) {
            bool isCalibrated = false;
            double matrix[16];
            int eulerOrder = 11;
            double rotX = 0.0, rotY = 0.0, rotZ = 0.0;
            double approachOffset = 0.0;
            double offsetX = 0.0, offsetY = 0.0, offsetZ = 0.0;
            const bool hasMatrix = m_handEyeCalibWidget->getCalibData(camIdx, matrix, isCalibrated) && isCalibrated;
            const bool hasExtrinsic = m_handEyeCalibWidget->getExtrinsicData(
                camIdx, eulerOrder, rotX, rotY, rotZ, approachOffset, offsetX, offsetY, offsetZ);

            // 同一矩阵写入该相机的两个 set（screw + toolDisk）
            const DetectionType types[2] = { DETECTION_TYPE_SCREW, DETECTION_TYPE_TOOL_DISK };
            for (DetectionType t : types) {
                VrDetectionConfigSet* setPtr = systemConfig.configResult.FindDetectionConfig(camIdx, t);
                if (!setPtr) continue;

                setPtr->handEyeMatrix.cameraIndex = camIdx;
                if (hasMatrix) {
                    std::memcpy(setPtr->handEyeMatrix.matrix, matrix, sizeof(double) * 16);
                }
                if (hasExtrinsic) {
                    setPtr->handEyeMatrix.eulerOrder = eulerOrder;
                    setPtr->handEyeMatrix.rotX = rotX;
                    setPtr->handEyeMatrix.rotY = rotY;
                    setPtr->handEyeMatrix.rotZ = rotZ;
                    setPtr->handEyeMatrix.approachOffset = approachOffset;
                    setPtr->handEyeMatrix.offsetX = offsetX;
                    setPtr->handEyeMatrix.offsetY = offsetY;
                    setPtr->handEyeMatrix.offsetZ = offsetZ;
                }
            }
        }
    }

    if (m_networkConfigWidget) {
        const NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
        if (netConfig.tcpServerPort <= 0 || netConfig.tcpServerPort > 65535) {
            StyledMessageBox::warning(this, QStringLiteral("错误"),
                QStringLiteral("TCP端口必须在 1 到 65535 之间。"));
            return false;
        }

        systemConfig.configResult.tcpPort = static_cast<uint16_t>(netConfig.tcpServerPort);
        systemConfig.configResult.poseOutputOrder = netConfig.poseOutputOrder;
        systemConfig.configResult.byteOrder = netConfig.byteOrder;
    }

    if (!m_presenter->GetConfigManager()->UpdateFullConfig(systemConfig)) {
        StyledMessageBox::warning(this, QStringLiteral("失败"),
            QStringLiteral("更新配置缓存失败。"));
        return false;
    }

    const QString configPath = PathManager::GetInstance().GetConfigFilePath();
    if (!m_presenter->GetConfigManager()->SaveConfigToFile(configPath.toStdString())) {
        StyledMessageBox::warning(this, QStringLiteral("失败"),
            QStringLiteral("保存配置文件失败。"));
        return false;
    }

    m_presenter->OnConfigChanged(systemConfig.configResult);
    return true;
}

void DialogAlgoArg::resetParams()
{
    setDoubleEditorText(ui->spinRodDiameter, 10.0);
    ui->chkHorizonScan->setChecked(true);

    setDoubleEditorText(ui->spinCornerTh, 60.0);
    setDoubleEditorText(ui->spinScale, 50.0);
    setDoubleEditorText(ui->spinMinEndingGap, 20.0);
    setDoubleEditorText(ui->spinMinEndingGapZ, 20.0);
    setDoubleEditorText(ui->spinJumpCornerTh1, 10.0);
    setDoubleEditorText(ui->spinJumpCornerTh2, 60.0);

    setDoubleEditorText(ui->spinContinuityTh, 20.0);
    setDoubleEditorText(ui->spinOutlierTh, 5.0);

    setIntEditorText(ui->spinMaxLineSkipNum, 10);
    setDoubleEditorText(ui->spinYDeviationMax, 10.0);
    setDoubleEditorText(ui->spinMaxSkipDistance, 10.0);
    setDoubleEditorText(ui->spinZDeviationMax, 10.0);
    setDoubleEditorText(ui->spinMinLTypeTreeLen, 100.0);
    setDoubleEditorText(ui->spinMinVTypeTreeLen, 100.0);
}

void DialogAlgoArg::on_btnOK_clicked()
{
    if (saveParams()) {
        accept();
    }
}

void DialogAlgoArg::on_btnCancel_clicked()
{
    reject();
}

void DialogAlgoArg::on_btnApply_clicked()
{
    if (saveParams()) {
        StyledMessageBox::information(this, QStringLiteral("提示"),
            QStringLiteral("参数已应用。"));
    }
}

void DialogAlgoArg::on_btnReset_clicked()
{
    const auto ret = StyledMessageBox::question(this, QStringLiteral("确认重置"),
        QStringLiteral("确定要重置为默认参数吗？"), QMessageBox::Yes | QMessageBox::No);
    if (ret == QMessageBox::Yes) {
        resetParams();
    }
}

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

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

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