#include "CalibViewMainWindow.h"
#include "CalibDataWidget.h"
#include "CalibResultWidget.h"
#include "BatchVerifyDialog.h"
#include "MainWindow.h"
#include "VrEyeViewWidget.h"
#include "../../SpinBoxPasteHelper.h"
#include "IChessboardDetector.h"

#include <QMenuBar>
#include <QAction>
#include <QSplitter>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QMessageBox>
#include <QFileDialog>
#include <QFile>
#include <QTextStream>
#include <QLabel>
#include <QScrollArea>
#include <QSettings>
#include <QDateTime>

#include <algorithm>
#include <cmath>

namespace {

int parseCalibType(const QVariant& value)
{
    bool ok = false;
    const QString text = value.toString().trimmed();
    const int numericType = text.toInt(&ok);
    if (ok && numericType >= 0 && numericType <= 2) {
        return numericType;
    }

    QString normalized = text.toLower();
    normalized.remove('-');
    normalized.remove('_');
    normalized.remove(' ');

    if (normalized == "eyetohand") {
        return 0;
    }
    if (normalized == "eyeinhand") {
        return 1;
    }
    if (normalized == "tcp") {
        return 2;
    }

    return -1;
}

}

CalibViewMainWindow::CalibViewMainWindow(QWidget* parent)
    : QMainWindow(parent)
    , m_calib(nullptr)
    , m_dataWidget(nullptr)
    , m_resultWidget(nullptr)
    , m_sbTransformX(nullptr)
    , m_sbTransformY(nullptr)
    , m_sbTransformZ(nullptr)
    , m_sbRoll(nullptr)
    , m_sbPitch(nullptr)
    , m_sbYaw(nullptr)
    , m_sbDirXX(nullptr)
    , m_sbDirXY(nullptr)
    , m_sbDirXZ(nullptr)
    , m_sbDirYX(nullptr)
    , m_sbDirYY(nullptr)
    , m_sbDirYZ(nullptr)
    , m_sbDirZX(nullptr)
    , m_sbDirZY(nullptr)
    , m_sbDirZZ(nullptr)
    , m_cbAngleUnit(nullptr)
    , m_cbEulerOrder(nullptr)
    , m_btnEulerToMatrix(nullptr)
    , m_btnMatrixToEuler(nullptr)
    , m_btnTransform(nullptr)
    , m_logEdit(nullptr)
    , m_hasResult(false)
    , m_robotView(nullptr)
    , m_vrEyeView(nullptr)
{
    // 创建标定实例
    m_calib = CreateHandEyeCalibInstance();

    setupUI();
    createMenuBar();
    SpinBoxPasteHelper::install(this);

    setWindowTitle("CalibView - 手眼标定工具 - 1.0.0");
    resize(1400, 700);

    updateStatusBar("就绪");
}

CalibViewMainWindow::~CalibViewMainWindow()
{
    if (m_calib) {
        DestroyHandEyeCalibInstance(m_calib);
        m_calib = nullptr;
    }
}

QWidget* CalibViewMainWindow::createRightPanel()
{
    QWidget* rightPanel = new QWidget(this);
    QVBoxLayout* rightLayout = new QVBoxLayout(rightPanel);

    // 坐标变换测试组
    QGroupBox* transformGroup = new QGroupBox("坐标变换测试", this);
    QGridLayout* transformLayout = new QGridLayout(transformGroup);

    transformLayout->addWidget(new QLabel("X:", this), 0, 0);
    m_sbTransformX = new QDoubleSpinBox(this);
    m_sbTransformX->setRange(-10000, 10000);
    m_sbTransformX->setDecimals(3);
    transformLayout->addWidget(m_sbTransformX, 0, 1);

    transformLayout->addWidget(new QLabel("Y:", this), 0, 2);
    m_sbTransformY = new QDoubleSpinBox(this);
    m_sbTransformY->setRange(-10000, 10000);
    m_sbTransformY->setDecimals(3);
    transformLayout->addWidget(m_sbTransformY, 0, 3);

    transformLayout->addWidget(new QLabel("Z:", this), 0, 4);
    m_sbTransformZ = new QDoubleSpinBox(this);
    m_sbTransformZ->setRange(-10000, 10000);
    m_sbTransformZ->setDecimals(3);
    transformLayout->addWidget(m_sbTransformZ, 0, 5);

    // 第2行：姿态 Roll/Pitch/Yaw
    transformLayout->addWidget(new QLabel("Roll:", this), 1, 0);
    m_sbRoll = new QDoubleSpinBox(this);
    m_sbRoll->setRange(-180, 180);
    m_sbRoll->setDecimals(6);
    transformLayout->addWidget(m_sbRoll, 1, 1);

    transformLayout->addWidget(new QLabel("Pitch:", this), 1, 2);
    m_sbPitch = new QDoubleSpinBox(this);
    m_sbPitch->setRange(-180, 180);
    m_sbPitch->setDecimals(6);
    transformLayout->addWidget(m_sbPitch, 1, 3);

    transformLayout->addWidget(new QLabel("Yaw:", this), 1, 4);
    m_sbYaw = new QDoubleSpinBox(this);
    m_sbYaw->setRange(-180, 180);
    m_sbYaw->setDecimals(6);
    transformLayout->addWidget(m_sbYaw, 1, 5);

    // 第3行：角度单位与欧拉角顺序
    transformLayout->addWidget(new QLabel("角度单位:", this), 2, 0);
    m_cbAngleUnit = new QComboBox(this);
    m_cbAngleUnit->addItem("度(°)", 0);
    m_cbAngleUnit->addItem("弧度(rad)", 1);
    m_cbAngleUnit->setCurrentIndex(0);  // 默认度
    transformLayout->addWidget(m_cbAngleUnit, 2, 1, 1, 2);

    transformLayout->addWidget(new QLabel("欧拉角顺序:", this), 2, 3);
    m_cbEulerOrder = new QComboBox(this);
    m_cbEulerOrder->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
    m_cbEulerOrder->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
    m_cbEulerOrder->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
    m_cbEulerOrder->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
    m_cbEulerOrder->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
    m_cbEulerOrder->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
    m_cbEulerOrder->setCurrentIndex(5);  // 默认 ZYX
    transformLayout->addWidget(m_cbEulerOrder, 2, 4, 1, 2);

    // 第4行：X轴方向向量（x, y, z）
    transformLayout->addWidget(new QLabel("X轴(x,y,z):", this), 3, 0);
    m_sbDirXX = new QDoubleSpinBox(this);
    m_sbDirXX->setRange(-10000, 10000);
    m_sbDirXX->setDecimals(6);
    transformLayout->addWidget(m_sbDirXX, 3, 1);

    m_sbDirXY = new QDoubleSpinBox(this);
    m_sbDirXY->setRange(-10000, 10000);
    m_sbDirXY->setDecimals(6);
    transformLayout->addWidget(m_sbDirXY, 3, 3);

    m_sbDirXZ = new QDoubleSpinBox(this);
    m_sbDirXZ->setRange(-10000, 10000);
    m_sbDirXZ->setDecimals(6);
    transformLayout->addWidget(m_sbDirXZ, 3, 5);

    // 第5行：Y轴方向向量
    transformLayout->addWidget(new QLabel("Y轴(x,y,z):", this), 4, 0);
    m_sbDirYX = new QDoubleSpinBox(this);
    m_sbDirYX->setRange(-10000, 10000);
    m_sbDirYX->setDecimals(6);
    transformLayout->addWidget(m_sbDirYX, 4, 1);

    m_sbDirYY = new QDoubleSpinBox(this);
    m_sbDirYY->setRange(-10000, 10000);
    m_sbDirYY->setDecimals(6);
    transformLayout->addWidget(m_sbDirYY, 4, 3);

    m_sbDirYZ = new QDoubleSpinBox(this);
    m_sbDirYZ->setRange(-10000, 10000);
    m_sbDirYZ->setDecimals(6);
    transformLayout->addWidget(m_sbDirYZ, 4, 5);

    // 第6行：Z轴方向向量
    transformLayout->addWidget(new QLabel("Z轴(x,y,z):", this), 5, 0);
    m_sbDirZX = new QDoubleSpinBox(this);
    m_sbDirZX->setRange(-10000, 10000);
    m_sbDirZX->setDecimals(6);
    transformLayout->addWidget(m_sbDirZX, 5, 1);

    m_sbDirZY = new QDoubleSpinBox(this);
    m_sbDirZY->setRange(-10000, 10000);
    m_sbDirZY->setDecimals(6);
    transformLayout->addWidget(m_sbDirZY, 5, 3);

    m_sbDirZZ = new QDoubleSpinBox(this);
    m_sbDirZZ->setRange(-10000, 10000);
    m_sbDirZZ->setDecimals(6);
    transformLayout->addWidget(m_sbDirZZ, 5, 5);

    // 第7行：欧拉角与方向向量互转
    m_btnEulerToMatrix = new QPushButton("欧拉角 -> 向量", this);
    connect(m_btnEulerToMatrix, &QPushButton::clicked, this, &CalibViewMainWindow::onEulerToDirectionMatrix);
    transformLayout->addWidget(m_btnEulerToMatrix, 6, 0, 1, 3);

    m_btnMatrixToEuler = new QPushButton("向量 -> 欧拉角", this);
    connect(m_btnMatrixToEuler, &QPushButton::clicked, this, &CalibViewMainWindow::onDirectionMatrixToEuler);
    transformLayout->addWidget(m_btnMatrixToEuler, 6, 3, 1, 3);

    // 第8行：变换按钮
    m_btnTransform = new QPushButton("变换", this);
    connect(m_btnTransform, &QPushButton::clicked, this, &CalibViewMainWindow::onTransformTest);
    transformLayout->addWidget(m_btnTransform, 7, 0, 1, 6);

    updateDirectionMatrixDisplay(HECRotationMatrix());

    rightLayout->addWidget(transformGroup);

    // 日志组
    QGroupBox* logGroup = new QGroupBox("日志", this);
    QVBoxLayout* logLayout = new QVBoxLayout(logGroup);

    m_logEdit = new QTextEdit(this);
    m_logEdit->setReadOnly(true);
    m_logEdit->setFont(QFont("Consolas", 9));

    logLayout->addWidget(m_logEdit);

    // 清除日志按钮
    QPushButton* btnClearLog = new QPushButton("清除日志", this);
    connect(btnClearLog, &QPushButton::clicked, m_logEdit, &QTextEdit::clear);
    logLayout->addWidget(btnClearLog);

    rightLayout->addWidget(logGroup, 1);  // stretch=1 让日志区占据剩余空间

    return rightPanel;
}

void CalibViewMainWindow::setupUI()
{
    // 创建中央控件
    QWidget* centralWidget = new QWidget(this);
    QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget);

    // 创建分割器
    QSplitter* splitter = new QSplitter(Qt::Horizontal, this);

    // 左侧面板：数据输入 + 标定结果
    QWidget* leftPanel = new QWidget(this);
    QVBoxLayout* leftLayout = new QVBoxLayout(leftPanel);

    // 数据输入（可滚动）
    QScrollArea* dataScroll = new QScrollArea(this);
    m_dataWidget = new CalibDataWidget(this);
    dataScroll->setWidget(m_dataWidget);
    dataScroll->setWidgetResizable(true);
    leftLayout->addWidget(dataScroll, 1);

    // 标定结果
    m_resultWidget = new CalibResultWidget(this);
    leftLayout->addWidget(m_resultWidget);

    splitter->addWidget(leftPanel);

    // 右侧面板：测试工具 + 日志
    QWidget* rightPanel = createRightPanel();
    splitter->addWidget(rightPanel);

    // 设置分割比例（右侧测试栏保持紧凑）
    splitter->setStretchFactor(0, 5);
    splitter->setStretchFactor(1, 1);

    // 设置初始大小
    splitter->setSizes(QList<int>() << 1180 << 220);

    mainLayout->addWidget(splitter);
    setCentralWidget(centralWidget);

    // 创建状态栏
    statusBar()->showMessage("就绪");

    // 连接 CalibDataWidget 的信号
    connect(m_dataWidget, &CalibDataWidget::requestEyeToHandCalib, this, &CalibViewMainWindow::onEyeToHandCalib);
    connect(m_dataWidget, &CalibDataWidget::requestEyeInHandCalib, this, &CalibViewMainWindow::onEyeInHandCalib);
    connect(m_dataWidget, &CalibDataWidget::requestTCPCalib,       this, &CalibViewMainWindow::onTCPCalib);
}

void CalibViewMainWindow::createMenuBar()
{
    // 文件菜单
    QMenu* fileMenu = menuBar()->addMenu("文件(&F)");

    QAction* actSave = fileMenu->addAction("保存结果(&S)");
    actSave->setShortcut(QKeySequence::Save);
    connect(actSave, &QAction::triggered, this, &CalibViewMainWindow::onSaveResult);

    QAction* actLoad = fileMenu->addAction("加载结果(&L)");
    actLoad->setShortcut(QKeySequence::Open);
    connect(actLoad, &QAction::triggered, this, &CalibViewMainWindow::onLoadResult);

    fileMenu->addSeparator();

    QAction* actSaveData = fileMenu->addAction("保存标定数据(&D)");
    actSaveData->setShortcut(QKeySequence(Qt::CTRL | Qt::SHIFT | Qt::Key_S));
    connect(actSaveData, &QAction::triggered, this, &CalibViewMainWindow::onSaveCalibData);

    QAction* actLoadData = fileMenu->addAction("加载标定数据(&A)");
    actLoadData->setShortcut(QKeySequence(Qt::CTRL | Qt::SHIFT | Qt::Key_O));
    connect(actLoadData, &QAction::triggered, this, &CalibViewMainWindow::onLoadCalibData);

    fileMenu->addSeparator();

    QAction* actExit = fileMenu->addAction("退出(&X)");
    actExit->setShortcut(QKeySequence::Quit);
    connect(actExit, &QAction::triggered, this, &QMainWindow::close);

    // 标定菜单
    QMenu* calibMenu = menuBar()->addMenu("标定(&C)");

    QAction* actEyeToHand = calibMenu->addAction("Eye-To-Hand 标定(&E)");
    connect(actEyeToHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeToHandCalib);

    QAction* actEyeInHand = calibMenu->addAction("Eye-In-Hand 标定(&I)");
    connect(actEyeInHand, &QAction::triggered, this, &CalibViewMainWindow::onEyeInHandCalib);

    QAction* actTCPCalib = calibMenu->addAction("TCP 标定(&P)");
    connect(actTCPCalib, &QAction::triggered, this, &CalibViewMainWindow::onTCPCalib);

    calibMenu->addSeparator();

    QAction* actTransform = calibMenu->addAction("坐标变换测试(&T)");
    connect(actTransform, &QAction::triggered, this, &CalibViewMainWindow::onTransformTest);

    calibMenu->addSeparator();

    QAction* actClear = calibMenu->addAction("清除所有(&C)");
    connect(actClear, &QAction::triggered, this, &CalibViewMainWindow::onClearAll);

    // 工具菜单
    QMenu* toolMenu = menuBar()->addMenu("工具(&T)");
    QAction* actRobotView = toolMenu->addAction("机器人控制(&R)");
    connect(actRobotView, &QAction::triggered, this, &CalibViewMainWindow::onOpenRobotView);

    QAction* actVrEyeView = toolMenu->addAction("相机标定板检测(&V)");
    connect(actVrEyeView, &QAction::triggered, this, &CalibViewMainWindow::onOpenVrEyeView);

    toolMenu->addSeparator();

    QAction* actBatchVerify = toolMenu->addAction("批量验证(&B)");
    actBatchVerify->setShortcut(QKeySequence(Qt::CTRL | Qt::Key_B));
    connect(actBatchVerify, &QAction::triggered, this, &CalibViewMainWindow::onOpenBatchVerify);

    // 帮助菜单
    QMenu* helpMenu = menuBar()->addMenu("帮助(&H)");

    QAction* actAbout = helpMenu->addAction("关于(&A)");
    connect(actAbout, &QAction::triggered, this, [this]() {
        QMessageBox::about(this, "关于 CalibView",
            "CalibView - 手眼标定工具\n\n"
            "用于测试 HandEyeCalib 模块的各项功能：\n"
            "- Eye-To-Hand 标定\n"
            "- Eye-In-Hand 标定\n"
            "- TCP 标定\n"
            "- 坐标变换\n"
            "- 方向向量与欧拉角互转\n\n"
            "基于 Eigen 库实现的 SVD 分解算法");
    });
}

void CalibViewMainWindow::updateStatusBar(const QString& message)
{
    statusBar()->showMessage(message);
}

void CalibViewMainWindow::appendLog(const QString& message)
{
    m_logEdit->append(message);
}

void CalibViewMainWindow::updateDirectionMatrixDisplay(const HECRotationMatrix& R)
{
    if (!m_sbDirXX || !m_sbDirXY || !m_sbDirXZ ||
        !m_sbDirYX || !m_sbDirYY || !m_sbDirYZ ||
        !m_sbDirZX || !m_sbDirZY || !m_sbDirZZ) {
        return;
    }

    m_sbDirXX->setValue(R.at(0, 0));
    m_sbDirXY->setValue(R.at(1, 0));
    m_sbDirXZ->setValue(R.at(2, 0));

    m_sbDirYX->setValue(R.at(0, 1));
    m_sbDirYY->setValue(R.at(1, 1));
    m_sbDirYZ->setValue(R.at(2, 1));

    m_sbDirZX->setValue(R.at(0, 2));
    m_sbDirZY->setValue(R.at(1, 2));
    m_sbDirZZ->setValue(R.at(2, 2));
}

bool CalibViewMainWindow::tryGetDirectionMatrixInput(HECRotationMatrix& R)
{
    if (!m_sbDirXX || !m_sbDirXY || !m_sbDirXZ ||
        !m_sbDirYX || !m_sbDirYY || !m_sbDirYZ ||
        !m_sbDirZX || !m_sbDirZY || !m_sbDirZZ) {
        return false;
    }

    auto dot = [](const HECPoint3D& a, const HECPoint3D& b) {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    };
    auto cross = [](const HECPoint3D& a, const HECPoint3D& b) {
        return HECPoint3D(
            a.y * b.z - a.z * b.y,
            a.z * b.x - a.x * b.z,
            a.x * b.y - a.y * b.x
        );
    };

    HECPoint3D xAxis(m_sbDirXX->value(), m_sbDirXY->value(), m_sbDirXZ->value());
    HECPoint3D yAxis(m_sbDirYX->value(), m_sbDirYY->value(), m_sbDirYZ->value());
    HECPoint3D zAxis(m_sbDirZX->value(), m_sbDirZY->value(), m_sbDirZZ->value());

    if (xAxis.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "X轴方向向量长度不能为 0");
        return false;
    }
    if (yAxis.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "Y轴方向向量长度不能为 0");
        return false;
    }
    if (zAxis.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "Z轴方向向量长度不能为 0");
        return false;
    }

    xAxis = xAxis.normalized();

    HECPoint3D yAxisOrtho = yAxis - xAxis * dot(xAxis, yAxis);
    if (yAxisOrtho.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "X轴与Y轴方向向量不能共线");
        return false;
    }
    yAxisOrtho = yAxisOrtho.normalized();

    HECPoint3D zAxisOrtho = cross(xAxis, yAxisOrtho);
    if (zAxisOrtho.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "无法根据 X/Y 轴方向向量构造正交坐标系");
        return false;
    }
    zAxisOrtho = zAxisOrtho.normalized();

    HECPoint3D zAxisNorm = zAxis.normalized();
    const double scoreKeep =
        dot(yAxisOrtho, yAxis.normalized()) + dot(zAxisOrtho, zAxisNorm);
    const double scoreFlip =
        dot(yAxisOrtho * (-1.0), yAxis.normalized()) + dot(zAxisOrtho * (-1.0), zAxisNorm);
    if (scoreFlip > scoreKeep) {
        yAxisOrtho = yAxisOrtho * (-1.0);
        zAxisOrtho = zAxisOrtho * (-1.0);
    }

    zAxisOrtho = zAxisOrtho - xAxis * dot(xAxis, zAxisOrtho);
    zAxisOrtho = zAxisOrtho - yAxisOrtho * dot(yAxisOrtho, zAxisOrtho);
    if (zAxisOrtho.norm() < 1e-10) {
        QMessageBox::warning(this, "警告", "输入的方向向量无法稳定构造正交坐标系");
        return false;
    }
    zAxisOrtho = zAxisOrtho.normalized();
    yAxisOrtho = cross(zAxisOrtho, xAxis).normalized();

    R.at(0, 0) = xAxis.x;
    R.at(1, 0) = xAxis.y;
    R.at(2, 0) = xAxis.z;
    R.at(0, 1) = yAxisOrtho.x;
    R.at(1, 1) = yAxisOrtho.y;
    R.at(2, 1) = yAxisOrtho.z;
    R.at(0, 2) = zAxisOrtho.x;
    R.at(1, 2) = zAxisOrtho.y;
    R.at(2, 2) = zAxisOrtho.z;

    return true;
}

QString CalibViewMainWindow::eulerOrderToString(HECEulerOrder order) const
{
    switch (order) {
    case HECEulerOrder::XYZ: return "XYZ";
    case HECEulerOrder::XZY: return "XZY";
    case HECEulerOrder::YXZ: return "YXZ";
    case HECEulerOrder::YZX: return "YZX";
    case HECEulerOrder::ZXY: return "ZXY";
    case HECEulerOrder::ZYX: return "ZYX";
    }

    return "ZYX";
}

void CalibViewMainWindow::onEyeToHandCalib()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    QMessageBox msgBox(this);
    msgBox.setWindowTitle("选择标定方法");
    msgBox.setText("请选择 Eye-To-Hand 标定方法：");
    QPushButton* simpleBtn = msgBox.addButton("简单方法 (仅位置)", QMessageBox::ActionRole);
    QPushButton* poseBtn = msgBox.addButton("完整位姿方法 (Park算法)", QMessageBox::ActionRole);
    QPushButton* cancelBtn = msgBox.addButton("取消", QMessageBox::RejectRole);

    msgBox.exec();

    if (msgBox.clickedButton() == cancelBtn) {
        return;
    }

    if (msgBox.clickedButton() == simpleBtn) {
        onEyeToHandCalibSimple();
    } else if (msgBox.clickedButton() == poseBtn) {
        onEyeToHandCalibWithPose();
    }
}

void CalibViewMainWindow::onEyeToHandCalibSimple()
{
    appendLog("开始 Eye-To-Hand 标定（简单方法 - 仅位置）...");

    std::vector<HECPoint3D> eyePoints;
    std::vector<HECPoint3D> robotPoints;
    m_dataWidget->getEyeToHandData(eyePoints, robotPoints);

    if (eyePoints.size() < 3) {
        QMessageBox::warning(this, "警告", "至少需要3组数据进行标定");
        return;
    }

    appendLog(QString("输入数据组数: %1").arg(eyePoints.size()));
    appendLog("使用 SVD 分解方法，仅利用位置信息");

    int ret = m_calib->CalculateRT(eyePoints, robotPoints, m_currentResult);

    if (ret == 0) {
        m_hasResult = true;
        m_resultWidget->showCalibResult(m_currentResult);
        appendLog(QString("标定成功，误差: %1 mm")
            .arg(m_currentResult.error, 0, 'f', 4));
        updateStatusBar("Eye-To-Hand 标定完成（简单方法）");
        emit calibrationCompleted(m_currentResult);
    } else {
        appendLog(QString("标定失败，错误码: %1").arg(ret));
        QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
    }
}

void CalibViewMainWindow::onEyeToHandCalibWithPose()
{
    appendLog("开始 Eye-To-Hand 标定（完整位姿方法 - Park 算法）...");

    std::vector<HECEyeToHandData> calibData;
    m_dataWidget->getEyeToHandPoseData(calibData);

    if (calibData.size() < 3) {
        QMessageBox::warning(this, "警告",
            "完整位姿法需要至少3组数据，且数据必须包含标定板的完整位姿信息。\n"
            "请确保数据已从包含完整位姿的 INI 文件加载。");
        return;
    }

    HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
    QString eulerOrderStr;
    switch (eulerOrder) {
    case HECEulerOrder::ZYX: eulerOrderStr = "ZYX"; break;
    case HECEulerOrder::XYZ: eulerOrderStr = "XYZ"; break;
    case HECEulerOrder::XZY: eulerOrderStr = "XZY"; break;
    case HECEulerOrder::YXZ: eulerOrderStr = "YXZ"; break;
    case HECEulerOrder::YZX: eulerOrderStr = "YZX"; break;
    case HECEulerOrder::ZXY: eulerOrderStr = "ZXY"; break;
    }

    appendLog(QString("输入数据组数: %1").arg(calibData.size()));
    appendLog(QString("欧拉角顺序: %1").arg(eulerOrderStr));
    appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）");

    int ret = m_calib->CalculateEyeToHandWithPose(calibData, m_currentResult);

    if (ret == 0) {
        m_hasResult = true;
        m_resultWidget->showCalibResult(m_currentResult);
        appendLog(QString("标定成功，误差: %1 mm")
            .arg(m_currentResult.error, 0, 'f', 4));
        updateStatusBar("Eye-To-Hand 标定完成（Park 方法）");
        emit calibrationCompleted(m_currentResult);
    } else {
        appendLog(QString("标定失败，错误码: %1").arg(ret));
        QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
    }
}

#if 0
void CalibViewMainWindow::onEyeInHandCalib()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    std::vector<HECEyeInHandPoseData> poseData;
    m_dataWidget->getEyeInHandPoseData(poseData);

    bool hasBoardPose = false;
    for (const auto& sample : poseData) {
        for (int r = 0; r < 3 && !hasBoardPose; ++r) {
            for (int c = 0; c < 3; ++c) {
                const double expected = (r == c) ? 1.0 : 0.0;
                if (std::abs(sample.boardInCamera.at(r, c) - expected) > 1e-6) {
                    hasBoardPose = true;
                    break;
                }
            }
        }
        if (hasBoardPose) {
            break;
        }
    }

    std::vector<HECEyeInHandData> pointData;
    if (!hasBoardPose) {
        m_dataWidget->getEyeInHandData(pointData);
    }

    /* legacy pose-calibration block kept commented out while rewriting around
    if (hasBoardPose) {
        appendLog("开始 Eye-In-Hand 标定...");
        appendLog(QString("输入数据组数: %1").arg(poseData.size()));

        const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
        QString eulerFormula;
        switch (eulerOrder) {
        case HECEulerOrder::XYZ: eulerFormula = "Rz*Ry*Rx"; break;
        case HECEulerOrder::XZY: eulerFormula = "Ry*Rz*Rx"; break;
        case HECEulerOrder::YXZ: eulerFormula = "Rz*Rx*Ry"; break;
        case HECEulerOrder::YZX: eulerFormula = "Rx*Rz*Ry"; break;
        case HECEulerOrder::ZXY: eulerFormula = "Ry*Rx*Rz"; break;
        case HECEulerOrder::ZYX: eulerFormula = "Rx*Ry*Rz"; break;
        }

        appendLog(QString("欧拉角顺序: %1 (R=%2)")
            .arg(eulerOrderToString(eulerOrder))
            .arg(eulerFormula));
        appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）");

        const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult);
        if (ret == 0) {
            m_hasResult = true;
            m_resultWidget->showCalibResult(m_currentResult);
#if 0
            appendLog(QString("标定成功，误差: %1 mm")
                .arg(m_currentResult.error, 0, 'f', 4));
            updateStatusBar("Eye-In-Hand 标定完成（Park 方法）");
#endif
            appendLog(QString("标定成功，误差: %1 mm")
                .arg(m_currentResult.error, 0, 'f', 4));
            updateStatusBar("Eye-In-Hand Park calibration finished");
            emit calibrationCompleted(m_currentResult);
        } else {
            appendLog(QString("标定失败，错误码: %1").arg(ret));
            QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
        }
        return;
    }
    */

    if (hasBoardPose) {
        appendLog("开始 Eye-In-Hand 标定...");
        appendLog(QString("输入数据组数: %1").arg(poseData.size()));

        const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
        QString eulerFormula;
        switch (eulerOrder) {
        case HECEulerOrder::XYZ: eulerFormula = "Rz*Ry*Rx"; break;
        case HECEulerOrder::XZY: eulerFormula = "Ry*Rz*Rx"; break;
        case HECEulerOrder::YXZ: eulerFormula = "Rz*Rx*Ry"; break;
        case HECEulerOrder::YZX: eulerFormula = "Rx*Rz*Ry"; break;
        case HECEulerOrder::ZXY: eulerFormula = "Ry*Rx*Rz"; break;
        case HECEulerOrder::ZYX: eulerFormula = "Rx*Ry*Rz"; break;
        }

        appendLog(QString("欧拉角顺序: %1 (R=%2)")
            .arg(eulerOrderToString(eulerOrder))
            .arg(eulerFormula));
        appendLog("使用 Park/Tsai-Lenz 算法，利用完整位姿信息（位置+姿态）");

        const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult);
        if (ret == 0) {
            m_hasResult = true;
            m_resultWidget->showCalibResult(m_currentResult);
            appendLog(QString("标定成功，误差: %1 mm")
                .arg(m_currentResult.error, 0, 'f', 4));

            if (m_currentResult.error > 20.0) {
                appendLog("当前误差仍然偏大，开始枚举机器人欧拉角顺序做诊断...");
                const HECEulerOrder probeOrders[] = {
                    HECEulerOrder::XYZ,
                    HECEulerOrder::XZY,
                    HECEulerOrder::YXZ,
                    HECEulerOrder::YZX,
                    HECEulerOrder::ZXY,
                    HECEulerOrder::ZYX
                };
                for (const HECEulerOrder probeOrder : probeOrders) {
                    std::vector<HECEyeInHandPoseData> probeData;
                    m_dataWidget->getEyeInHandPoseData(probeData, probeOrder);
                    if (probeData.size() < 3) {
                        continue;
                    }

                    HECCalibResult probeResult;
                    const int probeRet = m_calib->CalculateEyeInHandWithPose(probeData, probeResult);
                    if (probeRet == 0) {
                        appendLog(QString("顺序试探 %1 -> %2 mm")
                            .arg(eulerOrderToString(probeOrder))
                            .arg(probeResult.error, 0, 'f', 4));
                    } else {
                        appendLog(QString("顺序试探 %1 -> 失败(%2)")
                            .arg(eulerOrderToString(probeOrder))
                            .arg(probeRet));
                    }
                }
            }

#if 0
            updateStatusBar("Eye-In-Hand 标定完成（Park 方法）");
            emit calibrationCompleted(m_currentResult);
        } else {
            appendLog(QString("标定失败，错误码: %1").arg(ret));
            QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
        }
        return;
    }

#endif
            updateStatusBar("Eye-In-Hand Park calibration finished");
            emit calibrationCompleted(m_currentResult);
        } else {
            appendLog(QString("标定失败，错误码: %1").arg(ret));
            QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
        }
        return;
    }

    const std::vector<HECEyeInHandData>& calibData = pointData;

    if (calibData.size() < 3) {
        QMessageBox::warning(this, "警告", "至少需要3组数据进行标定");
        return;
    }

    appendLog("开始 Eye-In-Hand 标定...");
    appendLog(QString("输入数据组数: %1").arg(calibData.size()));

    HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
    const char* eulerOrderNames[] = {"XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX"};
    int orderIdx = static_cast<int>(eulerOrder);
    appendLog(QString("欧拉角顺序: %1 (R=%2)")
        .arg(eulerOrderNames[orderIdx])
        .arg(orderIdx == 0 ? "Rz*Ry*Rx" :
             orderIdx == 1 ? "Ry*Rz*Rx" :
             orderIdx == 2 ? "Rz*Rx*Ry" :
             orderIdx == 3 ? "Rx*Rz*Ry" :
             orderIdx == 4 ? "Ry*Rx*Rz" : "Rx*Ry*Rz"));

    int ret = m_calib->CalculateEyeInHand(calibData, m_currentResult);

    if (ret == 0) {
        m_hasResult = true;
        m_resultWidget->showCalibResult(m_currentResult);
        appendLog(QString("标定成功，误差: %1 mm")
            .arg(m_currentResult.error, 0, 'f', 4));
        updateStatusBar("Eye-In-Hand 标定完成");
        emit calibrationCompleted(m_currentResult);
    } else {
        appendLog(QString("标定失败，错误码: %1").arg(ret));
        QMessageBox::critical(this, "错误", QString("标定失败，错误码: %1").arg(ret));
    }
}

#endif

void CalibViewMainWindow::onEyeInHandCalib()
{
    if (!m_calib) {
        QMessageBox::critical(this, "Error", "Calibration instance is not initialized");
        return;
    }

    auto orderFormula = [](HECEulerOrder order) -> const char* {
        switch (order) {
        case HECEulerOrder::XYZ: return "Rz*Ry*Rx";
        case HECEulerOrder::XZY: return "Ry*Rz*Rx";
        case HECEulerOrder::YXZ: return "Rz*Rx*Ry";
        case HECEulerOrder::YZX: return "Rx*Rz*Ry";
        case HECEulerOrder::ZXY: return "Ry*Rx*Rz";
        case HECEulerOrder::ZYX: return "Rx*Ry*Rz";
        }
        return "unknown";
    };

    std::vector<HECEyeInHandPoseData> poseData;
    m_dataWidget->getEyeInHandPoseData(poseData);

    bool hasBoardPose = false;
    for (const auto& sample : poseData) {
        if (std::abs(sample.boardInCamera.at(0, 3)) > 1e-6 ||
            std::abs(sample.boardInCamera.at(1, 3)) > 1e-6 ||
            std::abs(sample.boardInCamera.at(2, 3)) > 1e-6) {
            hasBoardPose = true;
            break;
        }
        for (int r = 0; r < 3 && !hasBoardPose; ++r) {
            for (int c = 0; c < 3; ++c) {
                const double expected = (r == c) ? 1.0 : 0.0;
                if (std::abs(sample.boardInCamera.at(r, c) - expected) > 1e-6) {
                    hasBoardPose = true;
                    break;
                }
            }
        }
        if (hasBoardPose) {
            break;
        }
    }

    if (hasBoardPose) {
        const HECEulerOrder robotOrder = m_dataWidget->getEulerOrder();

        appendLog("Starting Eye-In-Hand calibration...");
        appendLog(QString("Input sample count: %1").arg(poseData.size()));
        appendLog(QString("Robot Euler order: %1 (%2)")
            .arg(eulerOrderToString(robotOrder))
            .arg(orderFormula(robotOrder)));
        appendLog("Board pose assumption: boardInCamera, Euler order = XYZ");
        appendLog("Algorithm: Park/Tsai-Lenz with full pose data");

        const int ret = m_calib->CalculateEyeInHandWithPose(poseData, m_currentResult);
        if (ret != 0) {
            appendLog(QString("Calibration failed, error code: %1").arg(ret));
            QMessageBox::critical(this, "Error", QString("Calibration failed, error code: %1").arg(ret));
            return;
        }

        m_hasResult = true;
        m_resultWidget->showCalibResult(m_currentResult);
        appendLog(QString("Calibration succeeded, error: %1 mm")
            .arg(m_currentResult.error, 0, 'f', 4));

        struct ProbeResult {
            HECEulerOrder robotOrder;
            HECEulerOrder boardOrder;
            bool invertBoardPose = false;
            double error = 0.0;
            double maxError = 0.0;
            double minError = 0.0;
            int ret = -1;
        };

        if (m_currentResult.error > 20.0) {
            appendLog("Running exhaustive Eye-In-Hand pose diagnostics...");
            const HECEulerOrder allOrders[] = {
                HECEulerOrder::XYZ,
                HECEulerOrder::XZY,
                HECEulerOrder::YXZ,
                HECEulerOrder::YZX,
                HECEulerOrder::ZXY,
                HECEulerOrder::ZYX
            };

            std::vector<ProbeResult> probeResults;
            probeResults.reserve(72);

            for (const HECEulerOrder probeRobotOrder : allOrders) {
                for (const HECEulerOrder probeBoardOrder : allOrders) {
                    for (const bool invertBoardPose : { false, true }) {
                        std::vector<HECEyeInHandPoseData> probeData;
                        m_dataWidget->getEyeInHandPoseData(
                            probeData,
                            probeRobotOrder,
                            probeBoardOrder,
                            invertBoardPose);
                        if (probeData.size() < 3) {
                            continue;
                        }

                        ProbeResult probe;
                        probe.robotOrder = probeRobotOrder;
                        probe.boardOrder = probeBoardOrder;
                        probe.invertBoardPose = invertBoardPose;

                        HECCalibResult probeCalibResult;
                        probe.ret = m_calib->CalculateEyeInHandWithPose(probeData, probeCalibResult);
                        if (probe.ret == 0) {
                            probe.error = probeCalibResult.error;
                            probe.maxError = probeCalibResult.maxError;
                            probe.minError = probeCalibResult.minError;
                        }

                        probeResults.push_back(probe);
                    }
                }
            }

            std::sort(probeResults.begin(), probeResults.end(),
                [](const ProbeResult& lhs, const ProbeResult& rhs) {
                    if (lhs.ret != 0 && rhs.ret != 0) {
                        return false;
                    }
                    if (lhs.ret != 0) {
                        return false;
                    }
                    if (rhs.ret != 0) {
                        return true;
                    }
                    return lhs.error < rhs.error;
                });

            int loggedCount = 0;
            for (const ProbeResult& probe : probeResults) {
                if (probe.ret != 0) {
                    continue;
                }

                appendLog(QString("Probe #%1: robot=%2(%3), board=%4(%5), pose=%6, err=%7 mm, max=%8, min=%9")
                    .arg(loggedCount + 1)
                    .arg(eulerOrderToString(probe.robotOrder))
                    .arg(orderFormula(probe.robotOrder))
                    .arg(eulerOrderToString(probe.boardOrder))
                    .arg(orderFormula(probe.boardOrder))
                    .arg(probe.invertBoardPose ? "cameraInBoard->inverse" : "boardInCamera")
                    .arg(probe.error, 0, 'f', 4)
                    .arg(probe.maxError, 0, 'f', 4)
                    .arg(probe.minError, 0, 'f', 4));

                ++loggedCount;
                if (loggedCount >= 12) {
                    break;
                }
            }
        }

        updateStatusBar("Eye-In-Hand pose calibration finished");
        emit calibrationCompleted(m_currentResult);
        return;
    }

    std::vector<HECEyeInHandData> calibData;
    m_dataWidget->getEyeInHandData(calibData);

    if (calibData.size() < 3) {
        QMessageBox::warning(this, "Warning", "At least 3 samples are required");
        return;
    }

    const HECEulerOrder eulerOrder = m_dataWidget->getEulerOrder();
    appendLog("Starting Eye-In-Hand point calibration...");
    appendLog(QString("Input sample count: %1").arg(calibData.size()));
    appendLog(QString("Robot Euler order: %1 (%2)")
        .arg(eulerOrderToString(eulerOrder))
        .arg(orderFormula(eulerOrder)));
    appendLog("Algorithm: fixed-target point method");

    const int ret = m_calib->CalculateEyeInHand(calibData, m_currentResult);
    if (ret == 0) {
        m_hasResult = true;
        m_resultWidget->showCalibResult(m_currentResult);
        appendLog(QString("Calibration succeeded, error: %1 mm")
            .arg(m_currentResult.error, 0, 'f', 4));
        updateStatusBar("Eye-In-Hand point calibration finished");
        emit calibrationCompleted(m_currentResult);
    } else {
        appendLog(QString("Calibration failed, error code: %1").arg(ret));
        QMessageBox::critical(this, "Error", QString("Calibration failed, error code: %1").arg(ret));
    }
}

void CalibViewMainWindow::onTCPCalib()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    HECTCPCalibData tcpData = m_dataWidget->getTCPCalibData();

    if (tcpData.poses.size() < 3) {
        QMessageBox::warning(this, "警告", "至少需要3组法兰位姿进行TCP标定");
        return;
    }

    if (tcpData.mode == HECTCPCalibMode::Full6DOF) {
        if (tcpData.referencePoseIndex < 0 ||
            tcpData.referencePoseIndex >= static_cast<int>(tcpData.poses.size())) {
            QMessageBox::warning(this, "警告",
                QString("参考位姿索引 %1 越界，有效范围: 0-%2")
                    .arg(tcpData.referencePoseIndex)
                    .arg(tcpData.poses.size() - 1));
            return;
        }
    }

    QString modeName = (tcpData.mode == HECTCPCalibMode::PositionOnly) ?
        "3-DOF 位置标定" : "6-DOF 完整标定";
    appendLog(QString("开始 TCP 标定 (%1)...").arg(modeName));
    appendLog(QString("输入位姿数: %1").arg(tcpData.poses.size()));

    HECTCPCalibResult tcpResult = m_calib->CalculateTCP(tcpData);

    if (tcpResult.success) {
        m_resultWidget->showTCPCalibResult(tcpResult);

        appendLog("=== TCP 标定结果 ===");
        appendLog(QString("TCP 位置偏移: tx=%1, ty=%2, tz=%3")
            .arg(tcpResult.tx, 0, 'f', 3)
            .arg(tcpResult.ty, 0, 'f', 3)
            .arg(tcpResult.tz, 0, 'f', 3));
        if (tcpResult.rx != 0 || tcpResult.ry != 0 || tcpResult.rz != 0) {
            appendLog(QString("TCP 姿态偏移: rx=%1\302\260, ry=%2\302\260, rz=%3\302\260")
                .arg(tcpResult.rx, 0, 'f', 2)
                .arg(tcpResult.ry, 0, 'f', 2)
                .arg(tcpResult.rz, 0, 'f', 2));
        }
        appendLog(QString("残差误差: %1 mm").arg(tcpResult.residualError, 0, 'f', 4));
        appendLog("TCP 标定成功");
        updateStatusBar("TCP 标定完成");
    } else {
        appendLog(QString("TCP 标定失败: %1")
            .arg(QString::fromStdString(tcpResult.errorMessage)));
        QMessageBox::critical(this, "错误",
            QString("TCP 标定失败: %1").arg(QString::fromStdString(tcpResult.errorMessage)));
    }
}

void CalibViewMainWindow::onEulerToDirectionMatrix()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    const bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1);
    const HECEulerAngles srcAngles = isRadian
        ? HECEulerAngles(m_sbRoll->value(), m_sbPitch->value(), m_sbYaw->value())
        : HECEulerAngles::fromDegrees(m_sbRoll->value(), m_sbPitch->value(), m_sbYaw->value());
    const HECEulerOrder order = static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());

    HECRotationMatrix rotation;
    m_calib->EulerToRotationMatrix(srcAngles, order, rotation);
    updateDirectionMatrixDisplay(rotation);

    double rollDeg = 0.0;
    double pitchDeg = 0.0;
    double yawDeg = 0.0;
    srcAngles.toDegrees(rollDeg, pitchDeg, yawDeg);

    appendLog(QString("欧拉角 -> 方向向量，顺序: %1").arg(eulerOrderToString(order)));
    appendLog(QString("  Euler(rad): Roll=%1, Pitch=%2, Yaw=%3")
        .arg(srcAngles.roll, 0, 'f', 6)
        .arg(srcAngles.pitch, 0, 'f', 6)
        .arg(srcAngles.yaw, 0, 'f', 6));
    appendLog(QString("  Euler(deg): Roll=%1°, Pitch=%2°, Yaw=%3°")
        .arg(rollDeg, 0, 'f', 3)
        .arg(pitchDeg, 0, 'f', 3)
        .arg(yawDeg, 0, 'f', 3));
    appendLog(QString("  X轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 0), 0, 'f', 6)
        .arg(rotation.at(1, 0), 0, 'f', 6)
        .arg(rotation.at(2, 0), 0, 'f', 6));
    appendLog(QString("  Y轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 1), 0, 'f', 6)
        .arg(rotation.at(1, 1), 0, 'f', 6)
        .arg(rotation.at(2, 1), 0, 'f', 6));
    appendLog(QString("  Z轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 2), 0, 'f', 6)
        .arg(rotation.at(1, 2), 0, 'f', 6)
        .arg(rotation.at(2, 2), 0, 'f', 6));

    updateStatusBar("欧拉角转方向向量完成");
}

void CalibViewMainWindow::onDirectionMatrixToEuler()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    HECRotationMatrix rotation;
    if (!tryGetDirectionMatrixInput(rotation)) {
        return;
    }
    updateDirectionMatrixDisplay(rotation);

    const bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1);
    const HECEulerOrder order = static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());

    HECEulerAngles angles;
    m_calib->RotationMatrixToEuler(rotation, order, angles);

    if (isRadian) {
        m_sbRoll->setValue(angles.roll);
        m_sbPitch->setValue(angles.pitch);
        m_sbYaw->setValue(angles.yaw);
    } else {
        double rollDeg = 0.0;
        double pitchDeg = 0.0;
        double yawDeg = 0.0;
        angles.toDegrees(rollDeg, pitchDeg, yawDeg);
        m_sbRoll->setValue(rollDeg);
        m_sbPitch->setValue(pitchDeg);
        m_sbYaw->setValue(yawDeg);
    }

    double rollDeg = 0.0;
    double pitchDeg = 0.0;
    double yawDeg = 0.0;
    angles.toDegrees(rollDeg, pitchDeg, yawDeg);

    appendLog(QString("方向向量 -> 欧拉角，顺序: %1").arg(eulerOrderToString(order)));
    appendLog(QString("  X轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 0), 0, 'f', 6)
        .arg(rotation.at(1, 0), 0, 'f', 6)
        .arg(rotation.at(2, 0), 0, 'f', 6));
    appendLog(QString("  Y轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 1), 0, 'f', 6)
        .arg(rotation.at(1, 1), 0, 'f', 6)
        .arg(rotation.at(2, 1), 0, 'f', 6));
    appendLog(QString("  Z轴方向: [%1, %2, %3]")
        .arg(rotation.at(0, 2), 0, 'f', 6)
        .arg(rotation.at(1, 2), 0, 'f', 6)
        .arg(rotation.at(2, 2), 0, 'f', 6));
    appendLog(QString("  Euler(rad): Roll=%1, Pitch=%2, Yaw=%3")
        .arg(angles.roll, 0, 'f', 6)
        .arg(angles.pitch, 0, 'f', 6)
        .arg(angles.yaw, 0, 'f', 6));
    appendLog(QString("  Euler(deg): Roll=%1°, Pitch=%2°, Yaw=%3°")
        .arg(rollDeg, 0, 'f', 3)
        .arg(pitchDeg, 0, 'f', 3)
        .arg(yawDeg, 0, 'f', 3));

    updateStatusBar("方向向量转欧拉角完成");
}

void CalibViewMainWindow::onTransformTest()
{
    if (!m_calib) {
        QMessageBox::critical(this, "错误", "标定实例未初始化");
        return;
    }

    if (!m_hasResult) {
        QMessageBox::warning(this, "警告", "请先执行标定或加载标定结果");
        return;
    }

    // 源位置
    HECPoint3D srcPoint(
        m_sbTransformX->value(),
        m_sbTransformY->value(),
        m_sbTransformZ->value()
    );

    // 获取角度单位（0=度，1=弧度）
    bool isRadian = (m_cbAngleUnit->currentData().toInt() == 1);

    // 源姿态
    HECEulerAngles srcAngles;
    if (isRadian) {
        // 弧度直接使用
        srcAngles = HECEulerAngles(
            m_sbRoll->value(),
            m_sbPitch->value(),
            m_sbYaw->value()
        );
    } else {
        // 角度转弧度
        srcAngles = HECEulerAngles::fromDegrees(
            m_sbRoll->value(),
            m_sbPitch->value(),
            m_sbYaw->value()
        );
    }
    HECEulerOrder order = static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());

    // 欧拉角 -> 旋转矩阵
    HECRotationMatrix R_src;
    m_calib->EulerToRotationMatrix(srcAngles, order, R_src);
    updateDirectionMatrixDisplay(R_src);

    // 变换位置
    HECPoint3D dstPoint;
    m_calib->TransformPoint(m_currentResult.R, m_currentResult.T, srcPoint, dstPoint);

    // 变换姿态：R_dst = R_calib * R_src
    HECRotationMatrix R_dst = m_currentResult.R * R_src;

    // 旋转矩阵 -> 欧拉角
    HECEulerAngles dstAngles;
    m_calib->RotationMatrixToEuler(R_dst, order, dstAngles);

    // 输出日志
    appendLog(QString("坐标变换结果:"));
    appendLog(QString("  源位置: (%1, %2, %3)")
        .arg(srcPoint.x, 0, 'f', 3)
        .arg(srcPoint.y, 0, 'f', 3)
        .arg(srcPoint.z, 0, 'f', 3));

    if (isRadian) {
        appendLog(QString("  源姿态: Roll=%1rad, Pitch=%2rad, Yaw=%3rad")
            .arg(m_sbRoll->value(), 0, 'f', 6)
            .arg(m_sbPitch->value(), 0, 'f', 6)
            .arg(m_sbYaw->value(), 0, 'f', 6));
    } else {
        appendLog(QString("  源姿态: Roll=%1°, Pitch=%2°, Yaw=%3°")
            .arg(m_sbRoll->value(), 0, 'f', 2)
            .arg(m_sbPitch->value(), 0, 'f', 2)
            .arg(m_sbYaw->value(), 0, 'f', 2));
    }

    appendLog(QString("  目标位置: (%1, %2, %3)")
        .arg(dstPoint.x, 0, 'f', 3)
        .arg(dstPoint.y, 0, 'f', 3)
        .arg(dstPoint.z, 0, 'f', 3));

    // 目标姿态：同时输出弧度和角度
    double dstRoll, dstPitch, dstYaw;
    dstAngles.toDegrees(dstRoll, dstPitch, dstYaw);
    appendLog(QString("  目标姿态(弧度): Roll=%1rad, Pitch=%2rad, Yaw=%3rad")
        .arg(dstAngles.roll, 0, 'f', 6)
        .arg(dstAngles.pitch, 0, 'f', 6)
        .arg(dstAngles.yaw, 0, 'f', 6));
    appendLog(QString("  目标姿态(角度): Roll=%1°, Pitch=%2°, Yaw=%3°")
        .arg(dstRoll, 0, 'f', 2)
        .arg(dstPitch, 0, 'f', 2)
        .arg(dstYaw, 0, 'f', 2));

    updateStatusBar("坐标变换完成");
}


void CalibViewMainWindow::onClearAll()
{
    m_dataWidget->clearAll();
    m_resultWidget->clearAll();
    m_logEdit->clear();
    m_sbTransformX->setValue(0);
    m_sbTransformY->setValue(0);
    m_sbTransformZ->setValue(0);
    m_sbRoll->setValue(0);
    m_sbPitch->setValue(0);
    m_sbYaw->setValue(0);
    updateDirectionMatrixDisplay(HECRotationMatrix());
    m_hasResult = false;
    updateStatusBar("已清除所有数据");
}

void CalibViewMainWindow::onSaveResult()
{
    if (!m_hasResult) {
        QMessageBox::warning(this, "警告", "没有可保存的标定结果");
        return;
    }

    QString defaultName = QString("CalibResult_%1.ini")
        .arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"));

    QString fileName = QFileDialog::getSaveFileName(this,
        "保存标定结果", defaultName, "INI文件 (*.ini)");

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

    QSettings ini(fileName, QSettings::IniFormat);
    ini.setIniCodec("UTF-8");

    // [CommInfo]
    ini.beginGroup("CommInfo");
    ini.setValue("nMaxMaxMatrixNum", 8);
    ini.setValue("nExistMatrixNum", 1);
    ini.endGroup();

    // [CalibMatrixInfo_0] - 4x4 齐次矩阵
    ini.beginGroup("CalibMatrixInfo_0");
    ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));
    ini.setValue("nCalibPosIdx", 0);
    ini.setValue("nCalibType", 0);
    ini.setValue("nCalibMode", 0);
    ini.setValue("sCalibPosName", 0);
    ini.setValue("dError", m_currentResult.error);
    // 第0行: R[0,0] R[0,1] R[0,2] Tx
    ini.setValue("dCalibMatrix_0",  m_currentResult.R.data[0]);
    ini.setValue("dCalibMatrix_1",  m_currentResult.R.data[1]);
    ini.setValue("dCalibMatrix_2",  m_currentResult.R.data[2]);
    ini.setValue("dCalibMatrix_3",  m_currentResult.T.data[0]);
    // 第1行: R[1,0] R[1,1] R[1,2] Ty
    ini.setValue("dCalibMatrix_4",  m_currentResult.R.data[3]);
    ini.setValue("dCalibMatrix_5",  m_currentResult.R.data[4]);
    ini.setValue("dCalibMatrix_6",  m_currentResult.R.data[5]);
    ini.setValue("dCalibMatrix_7",  m_currentResult.T.data[1]);
    // 第2行: R[2,0] R[2,1] R[2,2] Tz
    ini.setValue("dCalibMatrix_8",  m_currentResult.R.data[6]);
    ini.setValue("dCalibMatrix_9",  m_currentResult.R.data[7]);
    ini.setValue("dCalibMatrix_10", m_currentResult.R.data[8]);
    ini.setValue("dCalibMatrix_11", m_currentResult.T.data[2]);
    // 第3行: 0 0 0 1
    ini.setValue("dCalibMatrix_12", 0.0);
    ini.setValue("dCalibMatrix_13", 0.0);
    ini.setValue("dCalibMatrix_14", 0.0);
    ini.setValue("dCalibMatrix_15", 1.0);
    ini.endGroup();

    appendLog(QString("结果已保存到: %1").arg(fileName));
    updateStatusBar("结果已保存");
}

void CalibViewMainWindow::onLoadResult()
{
    QString fileName = QFileDialog::getOpenFileName(this,
        "加载标定结果", "", "INI文件 (*.ini)");

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

    QSettings ini(fileName, QSettings::IniFormat);
    ini.setIniCodec("UTF-8");

    // 校验是否包含标定矩阵
    ini.beginGroup("CalibMatrixInfo_0");
    bool hasMatrix = ini.contains("dCalibMatrix_0");
    if (!hasMatrix) {
        ini.endGroup();
        QMessageBox::warning(this, "警告", "该文件不包含有效的标定结果");
        return;
    }

    // 加载 4x4 齐次矩阵 → 拆分为 R[3x3] + T[3x1]
    // 第0行
    m_currentResult.R.data[0] = ini.value("dCalibMatrix_0", 0).toDouble();
    m_currentResult.R.data[1] = ini.value("dCalibMatrix_1", 0).toDouble();
    m_currentResult.R.data[2] = ini.value("dCalibMatrix_2", 0).toDouble();
    m_currentResult.T.data[0] = ini.value("dCalibMatrix_3", 0).toDouble();
    // 第1行
    m_currentResult.R.data[3] = ini.value("dCalibMatrix_4", 0).toDouble();
    m_currentResult.R.data[4] = ini.value("dCalibMatrix_5", 0).toDouble();
    m_currentResult.R.data[5] = ini.value("dCalibMatrix_6", 0).toDouble();
    m_currentResult.T.data[1] = ini.value("dCalibMatrix_7", 0).toDouble();
    // 第2行
    m_currentResult.R.data[6] = ini.value("dCalibMatrix_8", 0).toDouble();
    m_currentResult.R.data[7] = ini.value("dCalibMatrix_9", 0).toDouble();
    m_currentResult.R.data[8] = ini.value("dCalibMatrix_10", 0).toDouble();
    m_currentResult.T.data[2] = ini.value("dCalibMatrix_11", 0).toDouble();
    ini.endGroup();

    // 加载误差
    ini.beginGroup("CommInfo");
    m_currentResult.error = ini.value("dError", 0).toDouble();
    ini.endGroup();

    // 加载质心
    ini.beginGroup("CenterInfo");
    m_currentResult.centerEye.x   = ini.value("dCenterEyeX", 0).toDouble();
    m_currentResult.centerEye.y   = ini.value("dCenterEyeY", 0).toDouble();
    m_currentResult.centerEye.z   = ini.value("dCenterEyeZ", 0).toDouble();
    m_currentResult.centerRobot.x = ini.value("dCenterRobotX", 0).toDouble();
    m_currentResult.centerRobot.y = ini.value("dCenterRobotY", 0).toDouble();
    m_currentResult.centerRobot.z = ini.value("dCenterRobotZ", 0).toDouble();
    ini.endGroup();

    m_hasResult = true;
    m_resultWidget->showCalibResult(m_currentResult);
    appendLog(QString("已加载标定结果: %1").arg(fileName));
    updateStatusBar("标定结果已加载");
}

void CalibViewMainWindow::onOpenRobotView()
{
    if (!m_robotView) {
        m_robotView = new MainWindow(this);
        connect(m_robotView, &MainWindow::tcpPoseUpdated,
                this, &CalibViewMainWindow::onRobotTcpPoseReceived);
    }
    m_robotView->show();
    m_robotView->raise();
    m_robotView->activateWindow();
}

void CalibViewMainWindow::onRobotTcpPoseReceived(
    double x, double y, double z, double rx, double ry, double rz)
{
    m_dataWidget->setRobotInput(x, y, z, rx, ry, rz);
    appendLog(QString("收到机器人数据: (%1, %2, %3, %4, %5, %6)")
        .arg(x, 0, 'f', 2).arg(y, 0, 'f', 2).arg(z, 0, 'f', 2)
        .arg(rx, 0, 'f', 2).arg(ry, 0, 'f', 2).arg(rz, 0, 'f', 2));
}

void CalibViewMainWindow::onOpenVrEyeView()
{
    if (!m_vrEyeView) {
        m_vrEyeView = new VrEyeViewWidget();  // 不设置父窗口，独立窗口

        // 连接信号槽
        connect(m_vrEyeView, &VrEyeViewWidget::chessboardDetected,
                this, [this](const ChessboardDetectionData& data) {
            if (data.detected) {
                onChessboardDetected(data.x, data.y, data.z, data.rx, data.ry, data.rz);
            }
        });

        // 设置为独立窗口
        m_vrEyeView->setWindowFlags(Qt::Window);
        m_vrEyeView->resize(900, 700);
        m_vrEyeView->setWindowTitle("相机标定板检测 - VrEyeView");
    }

    m_vrEyeView->show();
    m_vrEyeView->raise();
    m_vrEyeView->activateWindow();
}

void CalibViewMainWindow::onChessboardDetected(
    double x, double y, double z, double rx, double ry, double rz)
{
    // 将标定板检测结果作为相机坐标输入到数据控件
    m_dataWidget->setCameraInput(x, y, z, rx, ry, rz);
    appendLog(QString("收到标定板检测数据: 位置(%1, %2, %3) 姿态(%4°, %5°, %6°)")
        .arg(x, 0, 'f', 2).arg(y, 0, 'f', 2).arg(z, 0, 'f', 2)
        .arg(rx, 0, 'f', 2).arg(ry, 0, 'f', 2).arg(rz, 0, 'f', 2));
}

void CalibViewMainWindow::onSaveCalibData()
{
    // 根据当前模式生成默认文件名
    static const char* typeNames[] = { "EyeToHand", "EyeInHand", "TCP" };
    int typeIndex = m_dataWidget->getCalibTypeIndex();
    QString typeName = typeNames[typeIndex];

    QString defaultName = QString("CalibData_%1_%2.ini")
        .arg(typeName)
        .arg(QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss"));

    QString fileName = QFileDialog::getSaveFileName(this,
        "保存标定数据", defaultName, "INI文件 (*.ini)");

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

    m_dataWidget->saveCalibData(fileName);
    appendLog(QString("标定数据已保存到: %1").arg(fileName));
    updateStatusBar("标定数据已保存");
}

void CalibViewMainWindow::onLoadCalibData()
{
    QString fileName = QFileDialog::getOpenFileName(this,
        "加载标定数据", "", "INI文件 (*.ini)");

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

    // 校验文件是否包含标定数据
    QSettings check(fileName, QSettings::IniFormat);
    check.beginGroup("CommInfo");
    int calibType = parseCalibType(check.value("eCalibType"));
    if (calibType < 0) {
        calibType = check.value("eCalibTypeIndex", -1).toInt();
    }
    QString saveTime = check.value("sCalibTime").toString();
    check.endGroup();

    if (calibType < 0 || calibType > 2) {
        QMessageBox::warning(this, "警告", "该文件不包含有效的标定数据");
        return;
    }

    m_dataWidget->loadCalibData(fileName);

    static const char* typeNames[] = { "EyeToHand", "EyeInHand", "TCP" };
    appendLog(QString("已加载标定数据: %1 (保存时间: %2, 模式: %3)")
        .arg(fileName).arg(saveTime).arg(typeNames[calibType]));
    updateStatusBar("标定数据已加载");
}

void CalibViewMainWindow::onOpenBatchVerify()
{
    // 创建标定板检测器实例
    IChessboardDetector* detector = CreateChessboardDetectorInstance();
    if (!detector) {
        QMessageBox::critical(this, "错误", "无法创建标定板检测器实例");
        return;
    }

    // 创建批量验证对话框
    BatchVerifyDialog* dialog = new BatchVerifyDialog(detector, m_calib, this);
    dialog->setAttribute(Qt::WA_DeleteOnClose);
    dialog->show();

    appendLog("打开批量验证工具");
}