GrabBag/Tools/CalibView/Src/CalibResultWidget.cpp

#include "CalibResultWidget.h"

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

CalibResultWidget::CalibResultWidget(QWidget* parent)
    : QWidget(parent)
    , m_tableRotation(nullptr)
    , m_lblError(nullptr)
    , m_lblCenterEye(nullptr)
    , m_lblCenterRobot(nullptr)
    , m_hasResult(false)
{
    setupUI();
}

CalibResultWidget::~CalibResultWidget()
{
}

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

    mainLayout->addStretch();

    // 旋转矩阵
    mainLayout->addWidget(createRotationGroup());

    // 误差
    mainLayout->addWidget(createErrorGroup());
}

QGroupBox* CalibResultWidget::createRotationGroup()
{
    QGroupBox* group = new QGroupBox("变换矩阵 [R | T]", this);
    QVBoxLayout* layout = new QVBoxLayout(group);

    m_tableRotation = new QTableWidget(3, 4, this);
    m_tableRotation->setHorizontalHeaderLabels({"Col 0", "Col 1", "Col 2", "T"});
    m_tableRotation->setVerticalHeaderLabels({"Row 0", "Row 1", "Row 2"});
    m_tableRotation->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableRotation->verticalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableRotation->setEditTriggers(QAbstractItemView::NoEditTriggers);
    m_tableRotation->setMaximumHeight(120);

    // 初始化为单位矩阵 + 零平移
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            QTableWidgetItem* item = new QTableWidgetItem(i == j ? "1.000000" : "0.000000");
            item->setTextAlignment(Qt::AlignCenter);
            m_tableRotation->setItem(i, j, item);
        }
        // 第4列：T 向量，初始为 0
        QTableWidgetItem* tItem = new QTableWidgetItem("0.000000");
        tItem->setTextAlignment(Qt::AlignCenter);
        m_tableRotation->setItem(i, 3, tItem);
    }

    layout->addWidget(m_tableRotation);
    return group;
}

QGroupBox* CalibResultWidget::createErrorGroup()
{
    QGroupBox* group = new QGroupBox("标定信息", this);
    QGridLayout* layout = new QGridLayout(group);

    layout->addWidget(new QLabel("标定误差:", this), 0, 0);
    m_lblError = new QLabel("0.0000 mm", this);
    m_lblError->setStyleSheet("font-weight: bold; color: red;");
    layout->addWidget(m_lblError, 0, 1);

    layout->addWidget(new QLabel("眼坐标系质心:", this), 1, 0);
    m_lblCenterEye = new QLabel("(0.000, 0.000, 0.000)", this);
    layout->addWidget(m_lblCenterEye, 1, 1);

    layout->addWidget(new QLabel("机器人坐标系质心:", this), 2, 0);
    m_lblCenterRobot = new QLabel("(0.000, 0.000, 0.000)", this);
    layout->addWidget(m_lblCenterRobot, 2, 1);

    return group;
}

void CalibResultWidget::updateRotationDisplay(const HECRotationMatrix& R)
{
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            QTableWidgetItem* item = m_tableRotation->item(i, j);
            if (item) {
                item->setText(QString::number(R.at(i, j), 'f', 6));
            }
        }
    }
}

void CalibResultWidget::updateTranslationDisplay(const HECTranslationVector& T)
{
    for (int i = 0; i < 3; ++i) {
        QTableWidgetItem* item = m_tableRotation->item(i, 3);
        if (item) {
            item->setText(QString::number(T.at(i), 'f', 6));
        }
    }
}

void CalibResultWidget::showCalibResult(const HECCalibResult& result)
{
    m_currentResult = result;
    m_hasResult = true;

    // 更新旋转矩阵
    updateRotationDisplay(result.R);

    // 更新平移向量
    updateTranslationDisplay(result.T);

    // 更新误差
    m_lblError->setText(QString::number(result.error, 'f', 4) + " mm");

    // 更新质心
    m_lblCenterEye->setText(QString("(%1, %2, %3)")
        .arg(result.centerEye.x, 0, 'f', 3)
        .arg(result.centerEye.y, 0, 'f', 3)
        .arg(result.centerEye.z, 0, 'f', 3));

    m_lblCenterRobot->setText(QString("(%1, %2, %3)")
        .arg(result.centerRobot.x, 0, 'f', 3)
        .arg(result.centerRobot.y, 0, 'f', 3)
        .arg(result.centerRobot.z, 0, 'f', 3));
}

void CalibResultWidget::clearAll()
{
    // 重置旋转矩阵为单位矩阵
    HECRotationMatrix identity;
    updateRotationDisplay(identity);

    // 重置平移向量
    HECTranslationVector zero;
    updateTranslationDisplay(zero);

    // 重置误差和质心
    m_lblError->setText("0.0000 mm");
    m_lblCenterEye->setText("(0.000, 0.000, 0.000)");
    m_lblCenterRobot->setText("(0.000, 0.000, 0.000)");

    m_hasResult = false;
}

void CalibResultWidget::showTCPCalibResult(const HECTCPCalibResult& result)
{
    // 平移向量写入表格第4列
    if (m_tableRotation->item(0, 3))
        m_tableRotation->item(0, 3)->setText(QString::number(result.tx, 'f', 6));
    if (m_tableRotation->item(1, 3))
        m_tableRotation->item(1, 3)->setText(QString::number(result.ty, 'f', 6));
    if (m_tableRotation->item(2, 3))
        m_tableRotation->item(2, 3)->setText(QString::number(result.tz, 'f', 6));

    // 仅位置模式时旋转矩阵标识为 N/A
    if (result.rx == 0 && result.ry == 0 && result.rz == 0) {
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                QTableWidgetItem* item = m_tableRotation->item(i, j);
                if (item) {
                    item->setText("N/A");
                }
            }
        }
    }

    // 误差显示
    m_lblError->setText(QString::number(result.residualError, 'f', 4) + " mm");

    // 质心标签复用为 TCP 偏移信息
    m_lblCenterEye->setText(QString("TCP 偏移: (%1, %2, %3)")
        .arg(result.tx, 0, 'f', 3)
        .arg(result.ty, 0, 'f', 3)
        .arg(result.tz, 0, 'f', 3));
    m_lblCenterRobot->setText(QString("残差误差: %1 mm").arg(result.residualError, 0, 'f', 4));
}