GrabBag/App/RodAndBarPosition/RodAndBarPositionApp/dialogalgoarg.cpp

#include "dialogalgoarg.h"
#include "ui_dialogalgoarg.h"
#include "RodAndBarPositionPresenter.h"
#include "StyledMessageBox.h"
#include "HandEyeCalibWidget.h"
#include "NetworkConfigWidget.h"
#include "PathManager.h"
#include "VrLog.h"
#include <cstring>
#include <QFormLayout>
#include <QGroupBox>

namespace
{

/// 姿态输入/输出顺序 → 欧拉角旋转顺序 映射
int PoseOutputOrderToEulerOrder(int poseOrder)
{
    switch (poseOrder) {
    case 0: return 10;  // RX-RY-RZ → XYZ
    case 1: return 15;  // RX-RZ-RY → XZY
    case 2: return 13;  // RY-RX-RZ → YXZ
    case 3: return 14;  // RY-RZ-RX → YZX
    case 4: return 12;  // RZ-RX-RY → ZXY
    case 5: return 11;  // RZ-RY-RX → ZYX
    default: return 11; // 默认 ZYX
    }
}

/// 欧拉角旋转顺序 → 姿态输入/输出顺序 映射（UI 显示用）
int EulerOrderToPoseOutputOrder(int eulerOrder)
{
    switch (eulerOrder) {
    case 10: return 0;  // XYZ → RX-RY-RZ
    case 15: return 1;  // XZY → RX-RZ-RY
    case 13: return 2;  // YXZ → RY-RX-RZ
    case 14: return 3;  // YZX → RY-RZ-RX
    case 12: return 4;  // ZXY → RZ-RX-RY
    case 11: return 5;  // ZYX → RZ-RY-RX
    default: return 5;  // 默认 RZ-RY-RX
    }
}

} // namespace

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

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

void DialogAlgoArg::SetPresenter(RodAndBarPositionPresenter* presenter)
{
    m_presenter = presenter;
    loadParams();

    // 初始化手眼标定 tab
    InitHandEyeCalibTab();

    // 初始化网络配置 tab
    InitNetworkConfigTab();
}

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

    auto params = m_presenter->GetAlgoParams();

    // 棒材参数
    ui->spinRodDiameter->setValue(params.rodParam.rodDiameter);
    ui->spinRodLen->setValue(params.rodParam.rodLen);

    // 角点检测参数
    ui->spinCornerTh->setValue(params.cornerParam.cornerTh);
    ui->spinScale->setValue(params.cornerParam.scale);
    ui->spinMinEndingGap->setValue(params.cornerParam.minEndingGap);
    ui->spinMinEndingGapZ->setValue(params.cornerParam.minEndingGap_z);
    ui->spinJumpCornerTh1->setValue(params.cornerParam.jumpCornerTh_1);
    ui->spinJumpCornerTh2->setValue(params.cornerParam.jumpCornerTh_2);

    // 滤波参数
    ui->spinContinuityTh->setValue(params.filterParam.continuityTh);
    ui->spinOutlierTh->setValue(params.filterParam.outlierTh);

    // 树生长参数
    ui->spinMaxLineSkipNum->setValue(params.growParam.maxLineSkipNum);
    ui->spinYDeviationMax->setValue(params.growParam.yDeviation_max);
    ui->spinMaxSkipDistance->setValue(params.growParam.maxSkipDistance);
    ui->spinZDeviationMax->setValue(params.growParam.zDeviation_max);
    ui->spinMinLTypeTreeLen->setValue(params.growParam.minLTypeTreeLen);
    ui->spinMinVTypeTreeLen->setValue(params.growParam.minVTypeTreeLen);
}

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

    auto params = m_presenter->GetAlgoParams();

    // 棒材参数
    params.rodParam.rodDiameter = ui->spinRodDiameter->value();
    params.rodParam.rodLen = ui->spinRodLen->value();

    // 角点检测参数
    params.cornerParam.cornerTh = ui->spinCornerTh->value();
    params.cornerParam.scale = ui->spinScale->value();
    params.cornerParam.minEndingGap = ui->spinMinEndingGap->value();
    params.cornerParam.minEndingGap_z = ui->spinMinEndingGapZ->value();
    params.cornerParam.jumpCornerTh_1 = ui->spinJumpCornerTh1->value();
    params.cornerParam.jumpCornerTh_2 = ui->spinJumpCornerTh2->value();

    // 滤波参数
    params.filterParam.continuityTh = ui->spinContinuityTh->value();
    params.filterParam.outlierTh = ui->spinOutlierTh->value();

    // 树生长参数
    params.growParam.maxLineSkipNum = ui->spinMaxLineSkipNum->value();
    params.growParam.yDeviation_max = ui->spinYDeviationMax->value();
    params.growParam.maxSkipDistance = ui->spinMaxSkipDistance->value();
    params.growParam.zDeviation_max = ui->spinZDeviationMax->value();
    params.growParam.minLTypeTreeLen = ui->spinMinLTypeTreeLen->value();
    params.growParam.minVTypeTreeLen = ui->spinMinVTypeTreeLen->value();

    m_presenter->SetAlgoParams(params);

    // 保存手眼标定矩阵 + 网络配置到配置文件（统一获取一次配置，避免互相覆盖）
    if (m_presenter->GetConfigManager()) {
        ConfigManager* configManager = m_presenter->GetConfigManager();
        SystemConfig systemConfig = configManager->GetConfig();
        NetworkConfigData netConfig;
        bool hasNetworkConfig = false;

        if (m_networkConfigWidget) {
            netConfig = m_networkConfigWidget->getConfig();
            // 欧拉角旋转顺序由姿态输入/输出顺序决定
            netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
            hasNetworkConfig = true;
            systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
            systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
            systemConfig.configResult.byteOrder = netConfig.byteOrder;
            systemConfig.configResult.longAxisDir = netConfig.longAxisDir;
        }
        if (hasNetworkConfig) {
            for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
                calibMatrix.eulerOrder = netConfig.eulerOrder;
            }
        }

        // 保存手眼标定矩阵
        if (m_handEyeCalibWidget) {
            const auto& oldMatrixList = systemConfig.configResult.handEyeCalibMatrixList;
            std::vector<VrHandEyeCalibMatrix> newMatrixList;
            const auto& cameraList = m_presenter->GetCameraList();
            int cameraCount = std::max(1, static_cast<int>(cameraList.size()));

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

                // 先从已有配置中获取该相机的矩阵作为基础
                VrHandEyeCalibMatrix calibMatrix;
                calibMatrix.cameraIndex = camIdx;
                calibMatrix.eulerOrder = systemConfig.configResult.eulerOrder;
                for (const auto& old : oldMatrixList) {
                    if (old.cameraIndex == camIdx) {
                        calibMatrix = old;
                        break;
                    }
                }
                calibMatrix.eulerOrder = systemConfig.configResult.eulerOrder;

                // 如果控件中有更新的数据则覆盖
                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) {
            NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
            // 欧拉角旋转顺序由姿态输入/输出顺序决定
            netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);
            systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
            systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
            systemConfig.configResult.byteOrder = netConfig.byteOrder;
        }

        // 统一更新并保存到文件
        configManager->UpdateFullConfig(systemConfig);
        QString configPath = PathManager::GetInstance().GetConfigFilePath();
        configManager->SaveConfigToFile(configPath.toStdString());

        // 通知 Presenter 重新加载手眼标定矩阵等参数
        m_presenter->OnConfigChanged(systemConfig.configResult);
    }
}

void DialogAlgoArg::resetParams()
{
    ui->spinRodDiameter->setValue(52.0);
    ui->spinRodLen->setValue(290.0);

    ui->spinCornerTh->setValue(60.0);
    ui->spinScale->setValue(13.0);
    ui->spinMinEndingGap->setValue(20.0);
    ui->spinMinEndingGapZ->setValue(5.0);
    ui->spinJumpCornerTh1->setValue(15.0);
    ui->spinJumpCornerTh2->setValue(60.0);

    ui->spinContinuityTh->setValue(5.0);
    ui->spinOutlierTh->setValue(5.0);

    ui->spinMaxLineSkipNum->setValue(5);
    ui->spinYDeviationMax->setValue(10.0);
    ui->spinMaxSkipDistance->setValue(10.0);
    ui->spinZDeviationMax->setValue(10.0);
    ui->spinMinLTypeTreeLen->setValue(100.0);
    ui->spinMinVTypeTreeLen->setValue(100.0);
}

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

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

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

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

// ========== 手眼标定相关实现 ==========

void DialogAlgoArg::InitHandEyeCalibTab()
{
    if (!ui || !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) {
            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);

    saveParams();
    StyledMessageBox::information(this, "成功", "手眼标定参数已保存！");
}

// ========== 网络配置相关实现 ==========

void DialogAlgoArg::InitNetworkConfigTab()
{
    if (!ui || !m_presenter) return;

    // 创建网络配置控件：不显示PLC配置，不显示TCP配置
    m_networkConfigWidget = new NetworkConfigWidget(false, false, this);
    ui->verticalLayout_networkConfigHost->addWidget(m_networkConfigWidget);

    // 隐藏"欧拉角旋转顺序"和"输出欧拉角顺序"控件（本项目由姿态输入/输出顺序统一控制）
    {
        QList<QGroupBox*> groups = m_networkConfigWidget->findChildren<QGroupBox*>();
        for (QGroupBox* group : groups) {
            if (group->title() == QStringLiteral("通信参数")) {
                QFormLayout* form = qobject_cast<QFormLayout*>(group->layout());
                if (form) {
                    for (int i = 0; i < form->rowCount(); ++i) {
                        QLayoutItem* labelItem = form->itemAt(i, QFormLayout::LabelRole);
                        if (labelItem && labelItem->widget()) {
                            QLabel* label = qobject_cast<QLabel*>(labelItem->widget());
                            if (label && (label->text().contains(QStringLiteral("欧拉角旋转顺序"))
                                       || label->text().contains(QStringLiteral("输出欧拉角顺序")))) {
                                label->hide();
                                QLayoutItem* fieldItem = form->itemAt(i, QFormLayout::FieldRole);
                                if (fieldItem && fieldItem->widget()) {
                                    fieldItem->widget()->hide();
                                }
                            }
                        }
                    }
                }
                break;
            }
        }
    }

    // 加载当前配置
    loadNetworkConfig();
}

void DialogAlgoArg::loadNetworkConfig()
{
    if (!m_presenter || !m_networkConfigWidget) return;

    ConfigManager* configManager = m_presenter->GetConfigManager();
    if (!configManager) return;

    ConfigResult configResult = configManager->GetConfigResult();

    NetworkConfigData netConfig;
    // 欧拉角旋转顺序由姿态输入/输出顺序统一控制
    // 从存储的 eulerOrder 反推 poseOutputOrder 用于UI显示
    netConfig.eulerOrder = configResult.eulerOrder;
    if (!configResult.handEyeCalibMatrixList.empty()) {
        netConfig.eulerOrder = configResult.handEyeCalibMatrixList[0].eulerOrder;
    }
    netConfig.poseOutputOrder = EulerOrderToPoseOutputOrder(netConfig.eulerOrder);
    netConfig.dirVectorInvert = configResult.dirVectorInvert;
    netConfig.byteOrder = configResult.byteOrder;
    netConfig.longAxisDir = configResult.longAxisDir;

    m_networkConfigWidget->setConfig(netConfig);
}

void DialogAlgoArg::saveNetworkConfig()
{
    if (!m_presenter || !m_networkConfigWidget) return;

    ConfigManager* configManager = m_presenter->GetConfigManager();
    if (!configManager) return;

    NetworkConfigData netConfig = m_networkConfigWidget->getConfig();
    // 欧拉角旋转顺序由姿态输入/输出顺序决定
    netConfig.eulerOrder = PoseOutputOrderToEulerOrder(netConfig.poseOutputOrder);

    // 获取当前完整配置并更新网络参数
    SystemConfig systemConfig = configManager->GetConfig();
    systemConfig.configResult.eulerOrder = netConfig.eulerOrder;
    systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert;
    systemConfig.configResult.byteOrder = netConfig.byteOrder;
    systemConfig.configResult.longAxisDir = netConfig.longAxisDir;
    for (auto& calibMatrix : systemConfig.configResult.handEyeCalibMatrixList) {
        calibMatrix.eulerOrder = netConfig.eulerOrder;
    }

    // 更新并保存
    configManager->UpdateFullConfig(systemConfig);

    QString configPath = PathManager::GetInstance().GetConfigFilePath();
    configManager->SaveConfigToFile(configPath.toStdString());
}