GrabBag/Tools/CalibView/Src/CalibDataWidget.cpp

#include "CalibDataWidget.h"
#include "../../SpinBoxPasteHelper.h"

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QHeaderView>
#include <QLabel>

CalibDataWidget::CalibDataWidget(QWidget* parent)
    : QWidget(parent)
    , m_cbCalibType(nullptr)
    , m_tableEyeToHand(nullptr)
    , m_groupEyeToHand(nullptr)
    , m_tableEyeInHand(nullptr)
    , m_groupEyeInHand(nullptr)
    , m_btnEyeToHandAddRow(nullptr)
    , m_btnEyeToHandDeleteRow(nullptr)
    , m_btnEyeToHandCalib(nullptr)
    , m_btnEyeInHandAddRow(nullptr)
    , m_btnEyeInHandDeleteRow(nullptr)
    , m_btnEyeInHandCalib(nullptr)
    , m_groupTCPCalib(nullptr)
    , m_tableTCP(nullptr)
    , m_tcpModeCombo(nullptr)
    , m_tcpEulerOrderCombo(nullptr)
    , m_tcpRefPoseIndex(nullptr)
    , m_tcpWorldRx(nullptr)
    , m_tcpWorldRy(nullptr)
    , m_tcpWorldRz(nullptr)
    , m_tcpOrientationGroup(nullptr)
    , m_tcpAddRowBtn(nullptr)
    , m_tcpRemoveRowBtn(nullptr)
    , m_btnTCPCalib(nullptr)
    , m_inputEyeX(nullptr)
    , m_inputEyeY(nullptr)
    , m_inputEyeZ(nullptr)
    , m_inputRobotX(nullptr)
    , m_inputRobotY(nullptr)
    , m_inputRobotZ(nullptr)
    , m_btnEyeToHandAddInput(nullptr)
    , m_inputEndX(nullptr)
    , m_inputEndY(nullptr)
    , m_inputEndZ(nullptr)
    , m_inputEndRoll(nullptr)
    , m_inputEndPitch(nullptr)
    , m_inputEndYaw(nullptr)
    , m_inputCamX(nullptr)
    , m_inputCamY(nullptr)
    , m_inputCamZ(nullptr)
    , m_btnEyeInHandAddInput(nullptr)
    , m_inputTcpX(nullptr)
    , m_inputTcpY(nullptr)
    , m_inputTcpZ(nullptr)
    , m_inputTcpRx(nullptr)
    , m_inputTcpRy(nullptr)
    , m_inputTcpRz(nullptr)
    , m_btnTcpAddInput(nullptr)
{
    setupUI();
    SpinBoxPasteHelper::install(this);
}

CalibDataWidget::~CalibDataWidget()
{
}

void CalibDataWidget::setupUI()
{
    QVBoxLayout* mainLayout = new QVBoxLayout(this);

    // 标定模式选择
    QHBoxLayout* modeLayout = new QHBoxLayout();
    QLabel* lblMode = new QLabel("标定模式:", this);
    m_cbCalibType = new QComboBox(this);
    m_cbCalibType->addItem("Eye-To-Hand (眼在手外)");
    m_cbCalibType->addItem("Eye-In-Hand (眼在手上)");
    m_cbCalibType->addItem("TCP 标定");
    connect(m_cbCalibType, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &CalibDataWidget::onCalibTypeChanged);
    modeLayout->addWidget(lblMode);
    modeLayout->addWidget(m_cbCalibType);
    modeLayout->addStretch();
    mainLayout->addLayout(modeLayout);

    // Eye-To-Hand 数据组
    m_groupEyeToHand = createEyeToHandGroup();
    mainLayout->addWidget(m_groupEyeToHand, 1);

    // Eye-In-Hand 数据组
    m_groupEyeInHand = createEyeInHandGroup();
    mainLayout->addWidget(m_groupEyeInHand, 1);
    m_groupEyeInHand->setVisible(false);

    // TCP 标定数据组
    m_groupTCPCalib = createTCPCalibGroup();
    mainLayout->addWidget(m_groupTCPCalib, 1);
    m_groupTCPCalib->setVisible(false);

    // 初始化按钮启用状态
    onCalibTypeChanged(0);
}

QWidget* CalibDataWidget::createEyeToHandGroup()
{
    QWidget* group = new QWidget(this);
    QVBoxLayout* layout = new QVBoxLayout(group);
    layout->setContentsMargins(0, 0, 0, 0);

    m_tableEyeToHand = new QTableWidget(this);
    m_tableEyeToHand->setColumnCount(6);
    m_tableEyeToHand->setHorizontalHeaderLabels({
        "Eye X", "Eye Y", "Eye Z", "Robot X", "Robot Y", "Robot Z"
    });
    m_tableEyeToHand->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableEyeToHand->setSelectionBehavior(QAbstractItemView::SelectRows);

    layout->addWidget(m_tableEyeToHand, 1);

    // 内联按钮行
    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_btnEyeToHandAddRow = new QPushButton("添加行", this);
    m_btnEyeToHandDeleteRow = new QPushButton("删除行", this);
    m_btnEyeToHandCalib = new QPushButton("Eye-To-Hand 标定", this);

    connect(m_btnEyeToHandAddRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeToHand->rowCount();
        m_tableEyeToHand->insertRow(row);
        for (int col = 0; col < 6; ++col) {
            m_tableEyeToHand->setItem(row, col, new QTableWidgetItem("0"));
        }
        m_tableEyeToHand->scrollToBottom();
    });
    connect(m_btnEyeToHandDeleteRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeToHand->currentRow();
        if (row >= 0) {
            m_tableEyeToHand->removeRow(row);
        }
    });
    connect(m_btnEyeToHandCalib, &QPushButton::clicked, this, &CalibDataWidget::requestEyeToHandCalib);

    btnLayout->addWidget(m_btnEyeToHandAddRow);
    btnLayout->addWidget(m_btnEyeToHandDeleteRow);
    btnLayout->addWidget(m_btnEyeToHandCalib);
    btnLayout->addStretch();
    layout->addLayout(btnLayout);

    // 数据输入区
    QGroupBox* inputGroup = new QGroupBox("数据输入", group);
    QGridLayout* inputLayout = new QGridLayout(inputGroup);
    inputLayout->setHorizontalSpacing(2);
    inputLayout->setVerticalSpacing(4);
    inputLayout->setContentsMargins(4, 4, 4, 4);
    inputLayout->setColumnStretch(0, 0);
    inputLayout->setColumnStretch(1, 1);
    inputLayout->setColumnStretch(2, 0);
    inputLayout->setColumnStretch(3, 1);
    inputLayout->setColumnStretch(4, 0);
    inputLayout->setColumnStretch(5, 1);
    inputLayout->setColumnStretch(6, 0);

    auto createSpinBox = [this]() {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(-10000, 10000);
        sb->setDecimals(3);
        return sb;
    };

    // 第1行：Eye X/Y/Z
    inputLayout->addWidget(new QLabel("Eye X:", this), 0, 0);
    m_inputEyeX = createSpinBox();
    inputLayout->addWidget(m_inputEyeX, 0, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
    m_inputEyeY = createSpinBox();
    inputLayout->addWidget(m_inputEyeY, 0, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
    m_inputEyeZ = createSpinBox();
    inputLayout->addWidget(m_inputEyeZ, 0, 5);

    // 第2行：Robot X/Y/Z + 添加按钮
    inputLayout->addWidget(new QLabel("Robot X:", this), 1, 0);
    m_inputRobotX = createSpinBox();
    inputLayout->addWidget(m_inputRobotX, 1, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 1, 2);
    m_inputRobotY = createSpinBox();
    inputLayout->addWidget(m_inputRobotY, 1, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 1, 4);
    m_inputRobotZ = createSpinBox();
    inputLayout->addWidget(m_inputRobotZ, 1, 5);

    m_btnEyeToHandAddInput = new QPushButton("添加到表格", this);
    connect(m_btnEyeToHandAddInput, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeToHand->rowCount();
        m_tableEyeToHand->insertRow(row);
        m_tableEyeToHand->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputEyeX->value(), 'f', 3)));
        m_tableEyeToHand->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputEyeY->value(), 'f', 3)));
        m_tableEyeToHand->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputEyeZ->value(), 'f', 3)));
        m_tableEyeToHand->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputRobotX->value(), 'f', 3)));
        m_tableEyeToHand->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputRobotY->value(), 'f', 3)));
        m_tableEyeToHand->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputRobotZ->value(), 'f', 3)));
        m_tableEyeToHand->scrollToBottom();
    });
    inputLayout->addWidget(m_btnEyeToHandAddInput, 1, 6);

    layout->addWidget(inputGroup);

    return group;
}

QWidget* CalibDataWidget::createEyeInHandGroup()
{
    QWidget* group = new QWidget(this);
    QVBoxLayout* layout = new QVBoxLayout(group);
    layout->setContentsMargins(0, 0, 0, 0);

    m_tableEyeInHand = new QTableWidget(this);
    m_tableEyeInHand->setColumnCount(9);
    m_tableEyeInHand->setHorizontalHeaderLabels({
        "End X", "End Y", "End Z", "End Roll", "End Pitch", "End Yaw",
        "Cam X", "Cam Y", "Cam Z"
    });
    m_tableEyeInHand->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableEyeInHand->setSelectionBehavior(QAbstractItemView::SelectRows);

    layout->addWidget(m_tableEyeInHand, 1);

    // 内联按钮行
    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_btnEyeInHandAddRow = new QPushButton("添加行", this);
    m_btnEyeInHandDeleteRow = new QPushButton("删除行", this);
    m_btnEyeInHandCalib = new QPushButton("Eye-In-Hand 标定", this);

    connect(m_btnEyeInHandAddRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeInHand->rowCount();
        m_tableEyeInHand->insertRow(row);
        for (int col = 0; col < 9; ++col) {
            m_tableEyeInHand->setItem(row, col, new QTableWidgetItem("0"));
        }
        m_tableEyeInHand->scrollToBottom();
    });
    connect(m_btnEyeInHandDeleteRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeInHand->currentRow();
        if (row >= 0) {
            m_tableEyeInHand->removeRow(row);
        }
    });
    connect(m_btnEyeInHandCalib, &QPushButton::clicked, this, &CalibDataWidget::requestEyeInHandCalib);

    btnLayout->addWidget(m_btnEyeInHandAddRow);
    btnLayout->addWidget(m_btnEyeInHandDeleteRow);
    btnLayout->addWidget(m_btnEyeInHandCalib);
    btnLayout->addStretch();
    layout->addLayout(btnLayout);

    // 数据输入区
    QGroupBox* inputGroup = new QGroupBox("数据输入", group);
    QGridLayout* inputLayout = new QGridLayout(inputGroup);
    inputLayout->setHorizontalSpacing(2);
    inputLayout->setVerticalSpacing(4);
    inputLayout->setContentsMargins(4, 4, 4, 4);
    for (int c = 0; c <= 11; ++c)
        inputLayout->setColumnStretch(c, (c % 2 == 1) ? 1 : 0);

    auto createSpinBox = [this]() {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(-10000, 10000);
        sb->setDecimals(3);
        return sb;
    };

    // 第1行：末端位姿
    inputLayout->addWidget(new QLabel("End X:", this), 0, 0);
    m_inputEndX = createSpinBox();
    inputLayout->addWidget(m_inputEndX, 0, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
    m_inputEndY = createSpinBox();
    inputLayout->addWidget(m_inputEndY, 0, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
    m_inputEndZ = createSpinBox();
    inputLayout->addWidget(m_inputEndZ, 0, 5);
    inputLayout->addWidget(new QLabel("Roll\302\260:", this), 0, 6);
    m_inputEndRoll = createSpinBox();
    inputLayout->addWidget(m_inputEndRoll, 0, 7);
    inputLayout->addWidget(new QLabel("Pitch\302\260:", this), 0, 8);
    m_inputEndPitch = createSpinBox();
    inputLayout->addWidget(m_inputEndPitch, 0, 9);
    inputLayout->addWidget(new QLabel("Yaw\302\260:", this), 0, 10);
    m_inputEndYaw = createSpinBox();
    inputLayout->addWidget(m_inputEndYaw, 0, 11);

    // 第2行：相机观测点 + 添加按钮
    inputLayout->addWidget(new QLabel("Cam X:", this), 1, 0);
    m_inputCamX = createSpinBox();
    inputLayout->addWidget(m_inputCamX, 1, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 1, 2);
    m_inputCamY = createSpinBox();
    inputLayout->addWidget(m_inputCamY, 1, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 1, 4);
    m_inputCamZ = createSpinBox();
    inputLayout->addWidget(m_inputCamZ, 1, 5);

    m_btnEyeInHandAddInput = new QPushButton("添加到表格", this);
    connect(m_btnEyeInHandAddInput, &QPushButton::clicked, this, [this]() {
        int row = m_tableEyeInHand->rowCount();
        m_tableEyeInHand->insertRow(row);
        m_tableEyeInHand->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputEndX->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputEndY->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputEndZ->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputEndRoll->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputEndPitch->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputEndYaw->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 6, new QTableWidgetItem(QString::number(m_inputCamX->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 7, new QTableWidgetItem(QString::number(m_inputCamY->value(), 'f', 3)));
        m_tableEyeInHand->setItem(row, 8, new QTableWidgetItem(QString::number(m_inputCamZ->value(), 'f', 3)));
        m_tableEyeInHand->scrollToBottom();
    });
    inputLayout->addWidget(m_btnEyeInHandAddInput, 1, 10, 1, 2);

    layout->addWidget(inputGroup);

    return group;
}

void CalibDataWidget::updateTableVisibility()
{
    int index = m_cbCalibType->currentIndex();
    m_groupEyeToHand->setVisible(index == 0);
    m_groupEyeInHand->setVisible(index == 1);
    m_groupTCPCalib->setVisible(index == 2);
}

void CalibDataWidget::onCalibTypeChanged(int index)
{
    updateTableVisibility();

    // Eye-To-Hand 按钮
    m_btnEyeToHandCalib->setEnabled(index == 0);
    m_btnEyeToHandAddRow->setEnabled(index == 0);
    m_btnEyeToHandDeleteRow->setEnabled(index == 0);

    // Eye-In-Hand 按钮
    m_btnEyeInHandCalib->setEnabled(index == 1);
    m_btnEyeInHandAddRow->setEnabled(index == 1);
    m_btnEyeInHandDeleteRow->setEnabled(index == 1);

    // TCP 按钮
    m_tcpAddRowBtn->setEnabled(index == 2);
    m_tcpRemoveRowBtn->setEnabled(index == 2);
    m_btnTCPCalib->setEnabled(index == 2);

    if (index <= 1) {
        emit calibTypeChanged(index == 0 ? HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand);
    }
}

void CalibDataWidget::getEyeToHandData(std::vector<HECPoint3D>& eyePoints,
                                        std::vector<HECPoint3D>& robotPoints) const
{
    eyePoints.clear();
    robotPoints.clear();

    for (int row = 0; row < m_tableEyeToHand->rowCount(); ++row) {
        HECPoint3D eyePt, robotPt;

        QTableWidgetItem* item0 = m_tableEyeToHand->item(row, 0);
        QTableWidgetItem* item1 = m_tableEyeToHand->item(row, 1);
        QTableWidgetItem* item2 = m_tableEyeToHand->item(row, 2);
        QTableWidgetItem* item3 = m_tableEyeToHand->item(row, 3);
        QTableWidgetItem* item4 = m_tableEyeToHand->item(row, 4);
        QTableWidgetItem* item5 = m_tableEyeToHand->item(row, 5);

        if (item0 && item1 && item2 && item3 && item4 && item5) {
            eyePt.x = item0->text().toDouble();
            eyePt.y = item1->text().toDouble();
            eyePt.z = item2->text().toDouble();
            robotPt.x = item3->text().toDouble();
            robotPt.y = item4->text().toDouble();
            robotPt.z = item5->text().toDouble();

            eyePoints.push_back(eyePt);
            robotPoints.push_back(robotPt);
        }
    }
}

void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData) const
{
    calibData.clear();

    const double deg2rad = M_PI / 180.0;

    for (int row = 0; row < m_tableEyeInHand->rowCount(); ++row) {
        HECEyeInHandData data;

        // 获取末端位姿
        double endX = m_tableEyeInHand->item(row, 0) ?
            m_tableEyeInHand->item(row, 0)->text().toDouble() : 0;
        double endY = m_tableEyeInHand->item(row, 1) ?
            m_tableEyeInHand->item(row, 1)->text().toDouble() : 0;
        double endZ = m_tableEyeInHand->item(row, 2) ?
            m_tableEyeInHand->item(row, 2)->text().toDouble() : 0;
        double endRoll = m_tableEyeInHand->item(row, 3) ?
            m_tableEyeInHand->item(row, 3)->text().toDouble() * deg2rad : 0;
        double endPitch = m_tableEyeInHand->item(row, 4) ?
            m_tableEyeInHand->item(row, 4)->text().toDouble() * deg2rad : 0;
        double endYaw = m_tableEyeInHand->item(row, 5) ?
            m_tableEyeInHand->item(row, 5)->text().toDouble() * deg2rad : 0;

        // 构建末端位姿矩阵
        // 外旋 ZYX: R = Rx(roll) * Ry(pitch) * Rz(yaw)
        HECRotationMatrix R;
        double cr = cos(endRoll), sr = sin(endRoll);
        double cp = cos(endPitch), sp = sin(endPitch);
        double cy = cos(endYaw), sy = sin(endYaw);

        R.at(0, 0) = cp * cy;
        R.at(0, 1) = -cp * sy;
        R.at(0, 2) = sp;
        R.at(1, 0) = sr * sp * cy + cr * sy;
        R.at(1, 1) = -sr * sp * sy + cr * cy;
        R.at(1, 2) = -sr * cp;
        R.at(2, 0) = -cr * sp * cy + sr * sy;
        R.at(2, 1) = cr * sp * sy + sr * cy;
        R.at(2, 2) = cr * cp;

        HECTranslationVector T(endX, endY, endZ);
        data.endPose = HECHomogeneousMatrix(R, T);

        // 获取相机观测点
        data.targetInCamera.x = m_tableEyeInHand->item(row, 6) ?
            m_tableEyeInHand->item(row, 6)->text().toDouble() : 0;
        data.targetInCamera.y = m_tableEyeInHand->item(row, 7) ?
            m_tableEyeInHand->item(row, 7)->text().toDouble() : 0;
        data.targetInCamera.z = m_tableEyeInHand->item(row, 8) ?
            m_tableEyeInHand->item(row, 8)->text().toDouble() : 0;

        calibData.push_back(data);
    }
}

HECCalibrationType CalibDataWidget::getCalibType() const
{
    return m_cbCalibType->currentIndex() == 0 ?
        HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand;
}

void CalibDataWidget::clearAll()
{
    m_tableEyeToHand->setRowCount(0);
    m_tableEyeInHand->setRowCount(0);
    m_tableTCP->setRowCount(0);
}

QWidget* CalibDataWidget::createTCPCalibGroup()
{
    QWidget* group = new QWidget(this);
    QVBoxLayout* layout = new QVBoxLayout(group);
    layout->setContentsMargins(0, 0, 0, 0);

    // 模式选择行
    QHBoxLayout* modeLayout = new QHBoxLayout();
    modeLayout->addWidget(new QLabel("标定模式:", this));
    m_tcpModeCombo = new QComboBox(this);
    m_tcpModeCombo->addItem("位置标定 (3-DOF)");
    m_tcpModeCombo->addItem("完整标定 (6-DOF)");
    connect(m_tcpModeCombo, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &CalibDataWidget::onTCPModeChanged);
    modeLayout->addWidget(m_tcpModeCombo);

    modeLayout->addWidget(new QLabel("欧拉角顺序:", this));
    m_tcpEulerOrderCombo = new QComboBox(this);
    m_tcpEulerOrderCombo->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
    m_tcpEulerOrderCombo->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
    m_tcpEulerOrderCombo->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
    m_tcpEulerOrderCombo->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
    m_tcpEulerOrderCombo->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
    m_tcpEulerOrderCombo->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
    m_tcpEulerOrderCombo->setCurrentIndex(5);
    modeLayout->addWidget(m_tcpEulerOrderCombo);
    modeLayout->addStretch();
    layout->addLayout(modeLayout);

    // 法兰位姿表格
    m_tableTCP = new QTableWidget(this);
    m_tableTCP->setColumnCount(6);
    m_tableTCP->setHorizontalHeaderLabels({"X", "Y", "Z", "Roll(°)", "Pitch(°)", "Yaw(°)"});
    m_tableTCP->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableTCP->setSelectionBehavior(QAbstractItemView::SelectRows);
    layout->addWidget(m_tableTCP, 1);

    // 按钮行（含 TCP 标定按钮）
    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_tcpAddRowBtn = new QPushButton("添加行", this);
    m_tcpRemoveRowBtn = new QPushButton("删除行", this);
    m_btnTCPCalib = new QPushButton("TCP 标定", this);
    connect(m_tcpAddRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPAddRow);
    connect(m_tcpRemoveRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPRemoveRow);
    connect(m_btnTCPCalib, &QPushButton::clicked, this, &CalibDataWidget::requestTCPCalib);
    btnLayout->addWidget(m_tcpAddRowBtn);
    btnLayout->addWidget(m_tcpRemoveRowBtn);
    btnLayout->addWidget(m_btnTCPCalib);
    btnLayout->addStretch();
    layout->addLayout(btnLayout);

    // 数据输入区
    QGroupBox* inputGroup = new QGroupBox("数据输入", group);
    QGridLayout* inputLayout = new QGridLayout(inputGroup);
    inputLayout->setHorizontalSpacing(2);
    inputLayout->setVerticalSpacing(4);
    inputLayout->setContentsMargins(4, 4, 4, 4);
    inputLayout->setColumnStretch(0, 0);
    inputLayout->setColumnStretch(1, 1);
    inputLayout->setColumnStretch(2, 0);
    inputLayout->setColumnStretch(3, 1);
    inputLayout->setColumnStretch(4, 0);
    inputLayout->setColumnStretch(5, 1);
    inputLayout->setColumnStretch(6, 0);

    auto createSpinBox = [this]() {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(-10000, 10000);
        sb->setDecimals(3);
        return sb;
    };

    // 第1行：X/Y/Z
    inputLayout->addWidget(new QLabel("X:", this), 0, 0);
    m_inputTcpX = createSpinBox();
    inputLayout->addWidget(m_inputTcpX, 0, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
    m_inputTcpY = createSpinBox();
    inputLayout->addWidget(m_inputTcpY, 0, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
    m_inputTcpZ = createSpinBox();
    inputLayout->addWidget(m_inputTcpZ, 0, 5);

    // 第2行：Roll/Pitch/Yaw + 添加按钮
    inputLayout->addWidget(new QLabel("Roll\302\260:", this), 1, 0);
    m_inputTcpRx = createSpinBox();
    inputLayout->addWidget(m_inputTcpRx, 1, 1);
    inputLayout->addWidget(new QLabel("Pitch\302\260:", this), 1, 2);
    m_inputTcpRy = createSpinBox();
    inputLayout->addWidget(m_inputTcpRy, 1, 3);
    inputLayout->addWidget(new QLabel("Yaw\302\260:", this), 1, 4);
    m_inputTcpRz = createSpinBox();
    inputLayout->addWidget(m_inputTcpRz, 1, 5);

    m_btnTcpAddInput = new QPushButton("添加到表格", this);
    connect(m_btnTcpAddInput, &QPushButton::clicked, this, [this]() {
        int row = m_tableTCP->rowCount();
        m_tableTCP->insertRow(row);
        m_tableTCP->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputTcpX->value(), 'f', 3)));
        m_tableTCP->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputTcpY->value(), 'f', 3)));
        m_tableTCP->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputTcpZ->value(), 'f', 3)));
        m_tableTCP->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputTcpRx->value(), 'f', 3)));
        m_tableTCP->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputTcpRy->value(), 'f', 3)));
        m_tableTCP->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputTcpRz->value(), 'f', 3)));
        m_tableTCP->scrollToBottom();
    });
    inputLayout->addWidget(m_btnTcpAddInput, 1, 6);

    layout->addWidget(inputGroup);

    // 6-DOF 姿态参数组（默认隐藏）
    m_tcpOrientationGroup = new QGroupBox("6-DOF 姿态参数", this);
    QGridLayout* oriLayout = new QGridLayout(m_tcpOrientationGroup);

    oriLayout->addWidget(new QLabel("参考位姿索引:", this), 0, 0);
    m_tcpRefPoseIndex = new QSpinBox(this);
    m_tcpRefPoseIndex->setRange(0, 999);
    m_tcpRefPoseIndex->setValue(0);
    oriLayout->addWidget(m_tcpRefPoseIndex, 0, 1);

    oriLayout->addWidget(new QLabel("期望世界朝向 Rx(°):", this), 1, 0);
    m_tcpWorldRx = new QDoubleSpinBox(this);
    m_tcpWorldRx->setRange(-360, 360);
    m_tcpWorldRx->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRx, 1, 1);

    oriLayout->addWidget(new QLabel("Ry(°):", this), 1, 2);
    m_tcpWorldRy = new QDoubleSpinBox(this);
    m_tcpWorldRy->setRange(-360, 360);
    m_tcpWorldRy->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRy, 1, 3);

    oriLayout->addWidget(new QLabel("Rz(°):", this), 1, 4);
    m_tcpWorldRz = new QDoubleSpinBox(this);
    m_tcpWorldRz->setRange(-360, 360);
    m_tcpWorldRz->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRz, 1, 5);

    m_tcpOrientationGroup->setVisible(false);
    layout->addWidget(m_tcpOrientationGroup);

    return group;
}

void CalibDataWidget::onTCPModeChanged(int index)
{
    m_tcpOrientationGroup->setVisible(index == 1);
}

void CalibDataWidget::onTCPAddRow()
{
    int row = m_tableTCP->rowCount();
    m_tableTCP->insertRow(row);
    for (int col = 0; col < 6; ++col) {
        QTableWidgetItem* item = new QTableWidgetItem("0");
        m_tableTCP->setItem(row, col, item);
    }
    m_tableTCP->scrollToBottom();
}

void CalibDataWidget::onTCPRemoveRow()
{
    int row = m_tableTCP->currentRow();
    if (row >= 0) {
        m_tableTCP->removeRow(row);
    }
}

HECTCPCalibData CalibDataWidget::getTCPCalibData() const
{
    HECTCPCalibData data;

    // 标定模式
    data.mode = (m_tcpModeCombo->currentIndex() == 0) ?
        HECTCPCalibMode::PositionOnly : HECTCPCalibMode::Full6DOF;

    // 欧拉角顺序
    HECEulerOrder eulerOrder = static_cast<HECEulerOrder>(
        m_tcpEulerOrderCombo->currentData().toInt());

    // 读取表格中的法兰位姿
    for (int row = 0; row < m_tableTCP->rowCount(); ++row) {
        HECTCPCalibPose pose;
        pose.x  = m_tableTCP->item(row, 0) ? m_tableTCP->item(row, 0)->text().toDouble() : 0;
        pose.y  = m_tableTCP->item(row, 1) ? m_tableTCP->item(row, 1)->text().toDouble() : 0;
        pose.z  = m_tableTCP->item(row, 2) ? m_tableTCP->item(row, 2)->text().toDouble() : 0;
        pose.rx = m_tableTCP->item(row, 3) ? m_tableTCP->item(row, 3)->text().toDouble() : 0;
        pose.ry = m_tableTCP->item(row, 4) ? m_tableTCP->item(row, 4)->text().toDouble() : 0;
        pose.rz = m_tableTCP->item(row, 5) ? m_tableTCP->item(row, 5)->text().toDouble() : 0;
        pose.eulerOrder = eulerOrder;
        data.poses.push_back(pose);
    }

    // 6-DOF 参数
    data.referencePoseIndex = m_tcpRefPoseIndex->value();
    data.worldRx = m_tcpWorldRx->value();
    data.worldRy = m_tcpWorldRy->value();
    data.worldRz = m_tcpWorldRz->value();
    data.worldEulerOrder = eulerOrder;

    return data;
}

void CalibDataWidget::setRobotInput(double x, double y, double z,
                                     double rx, double ry, double rz)
{
    int mode = m_cbCalibType->currentIndex();
    if (mode == 0) {
        // Eye-To-Hand: 仅填充 Robot X/Y/Z
        m_inputRobotX->setValue(x);
        m_inputRobotY->setValue(y);
        m_inputRobotZ->setValue(z);
    } else if (mode == 1) {
        // Eye-In-Hand: 填充末端位姿
        m_inputEndX->setValue(x);
        m_inputEndY->setValue(y);
        m_inputEndZ->setValue(z);
        m_inputEndRoll->setValue(rx);
        m_inputEndPitch->setValue(ry);
        m_inputEndYaw->setValue(rz);
    } else if (mode == 2) {
        // TCP: 填充位姿
        m_inputTcpX->setValue(x);
        m_inputTcpY->setValue(y);
        m_inputTcpZ->setValue(z);
        m_inputTcpRx->setValue(rx);
        m_inputTcpRy->setValue(ry);
        m_inputTcpRz->setValue(rz);
    }
}

void CalibDataWidget::setCameraInput(double x, double y, double z,
                                      double rx, double ry, double rz)
{
    int mode = m_cbCalibType->currentIndex();
    if (mode == 0) {
        // Eye-To-Hand: 填充相机坐标 Eye X/Y/Z
        m_inputEyeX->setValue(x);
        m_inputEyeY->setValue(y);
        m_inputEyeZ->setValue(z);
    } else if (mode == 1) {
        // Eye-In-Hand: 填充相机观测点
        m_inputCamX->setValue(x);
        m_inputCamY->setValue(y);
        m_inputCamZ->setValue(z);
    }
    // TCP 模式不需要相机输入
}