#include "CalibResultWidget.h"

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

CalibResultWidget::CalibResultWidget(QWidget* parent)
    : QWidget(parent)
    , m_tableRotation(nullptr)
    , m_lblTransX(nullptr)
    , m_lblTransY(nullptr)
    , m_lblTransZ(nullptr)
    , m_lblRoll(nullptr)
    , m_lblPitch(nullptr)
    , m_lblYaw(nullptr)
    , m_lblRollDeg(nullptr)
    , m_lblPitchDeg(nullptr)
    , m_lblYawDeg(nullptr)
    , m_lblError(nullptr)
    , m_lblCenterEye(nullptr)
    , m_lblCenterRobot(nullptr)
    , m_logEdit(nullptr)
    , m_hasResult(false)
{
    setupUI();
}

CalibResultWidget::~CalibResultWidget()
{
}

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

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

    // 平移向量
    mainLayout->addWidget(createTranslationGroup());

    // 欧拉角
    mainLayout->addWidget(createEulerGroup());

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

    // 日志
    mainLayout->addWidget(createLogGroup(), 1);
}

QGroupBox* CalibResultWidget::createRotationGroup()
{
    QGroupBox* group = new QGroupBox("旋转矩阵 R", this);
    QVBoxLayout* layout = new QVBoxLayout(group);

    m_tableRotation = new QTableWidget(3, 3, this);
    m_tableRotation->setHorizontalHeaderLabels({"Col 0", "Col 1", "Col 2"});
    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);
        }
    }

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

QGroupBox* CalibResultWidget::createTranslationGroup()
{
    QGroupBox* group = new QGroupBox("平移向量 T", this);
    QGridLayout* layout = new QGridLayout(group);

    layout->addWidget(new QLabel("X:", this), 0, 0);
    m_lblTransX = new QLabel("0.000", this);
    m_lblTransX->setStyleSheet("font-weight: bold;");
    layout->addWidget(m_lblTransX, 0, 1);

    layout->addWidget(new QLabel("Y:", this), 0, 2);
    m_lblTransY = new QLabel("0.000", this);
    m_lblTransY->setStyleSheet("font-weight: bold;");
    layout->addWidget(m_lblTransY, 0, 3);

    layout->addWidget(new QLabel("Z:", this), 0, 4);
    m_lblTransZ = new QLabel("0.000", this);
    m_lblTransZ->setStyleSheet("font-weight: bold;");
    layout->addWidget(m_lblTransZ, 0, 5);

    return group;
}

QGroupBox* CalibResultWidget::createEulerGroup()
{
    QGroupBox* group = new QGroupBox("欧拉角 (ZYX顺序)", this);
    QGridLayout* layout = new QGridLayout(group);

    // 弧度
    layout->addWidget(new QLabel("Roll (rad):", this), 0, 0);
    m_lblRoll = new QLabel("0.000000", this);
    layout->addWidget(m_lblRoll, 0, 1);

    layout->addWidget(new QLabel("Pitch (rad):", this), 0, 2);
    m_lblPitch = new QLabel("0.000000", this);
    layout->addWidget(m_lblPitch, 0, 3);

    layout->addWidget(new QLabel("Yaw (rad):", this), 0, 4);
    m_lblYaw = new QLabel("0.000000", this);
    layout->addWidget(m_lblYaw, 0, 5);

    // 角度
    layout->addWidget(new QLabel("Roll (°):", this), 1, 0);
    m_lblRollDeg = new QLabel("0.00", this);
    m_lblRollDeg->setStyleSheet("font-weight: bold; color: blue;");
    layout->addWidget(m_lblRollDeg, 1, 1);

    layout->addWidget(new QLabel("Pitch (°):", this), 1, 2);
    m_lblPitchDeg = new QLabel("0.00", this);
    m_lblPitchDeg->setStyleSheet("font-weight: bold; color: blue;");
    layout->addWidget(m_lblPitchDeg, 1, 3);

    layout->addWidget(new QLabel("Yaw (°):", this), 1, 4);
    m_lblYawDeg = new QLabel("0.00", this);
    m_lblYawDeg->setStyleSheet("font-weight: bold; color: blue;");
    layout->addWidget(m_lblYawDeg, 1, 5);

    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;
}

QGroupBox* CalibResultWidget::createLogGroup()
{
    QGroupBox* group = new QGroupBox("日志", this);
    QVBoxLayout* layout = new QVBoxLayout(group);

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

    layout->addWidget(m_logEdit);
    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)
{
    m_lblTransX->setText(QString::number(T.at(0), 'f', 3));
    m_lblTransY->setText(QString::number(T.at(1), 'f', 3));
    m_lblTransZ->setText(QString::number(T.at(2), 'f', 3));
}

void CalibResultWidget::updateEulerDisplay(const HECEulerAngles& angles)
{
    m_lblRoll->setText(QString::number(angles.roll, 'f', 6));
    m_lblPitch->setText(QString::number(angles.pitch, 'f', 6));
    m_lblYaw->setText(QString::number(angles.yaw, 'f', 6));

    double rollDeg, pitchDeg, yawDeg;
    angles.toDegrees(rollDeg, pitchDeg, yawDeg);
    m_lblRollDeg->setText(QString::number(rollDeg, 'f', 2));
    m_lblPitchDeg->setText(QString::number(pitchDeg, 'f', 2));
    m_lblYawDeg->setText(QString::number(yawDeg, 'f', 2));
}

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

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

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

    // 计算并更新欧拉角 (使用 ZYX 顺序)
    // 从旋转矩阵提取欧拉角
    HECEulerAngles angles;
    double sy = -result.R.at(2, 0);
    if (std::abs(sy) < 0.99999) {
        angles.pitch = std::asin(sy);
        angles.roll = std::atan2(result.R.at(2, 1), result.R.at(2, 2));
        angles.yaw = std::atan2(result.R.at(1, 0), result.R.at(0, 0));
    } else {
        // 万向锁情况
        angles.pitch = sy > 0 ? M_PI / 2 : -M_PI / 2;
        angles.roll = 0;
        angles.yaw = std::atan2(-result.R.at(0, 1), result.R.at(1, 1));
    }
    updateEulerDisplay(angles);

    // 更新误差
    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::showTransformResult(const HECPoint3D& srcPoint,
                                             const HECPoint3D& dstPoint)
{
    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));
    appendLog(QString("  目标点: (%1, %2, %3)")
        .arg(dstPoint.x, 0, 'f', 3)
        .arg(dstPoint.y, 0, 'f', 3)
        .arg(dstPoint.z, 0, 'f', 3));
}

void CalibResultWidget::showEulerResult(const HECEulerAngles& inputAngles,
                                         const HECRotationMatrix& R,
                                         const HECEulerAngles& outputAngles,
                                         HECEulerOrder order)
{
    // 更新旋转矩阵显示
    updateRotationDisplay(R);

    // 更新欧拉角显示
    updateEulerDisplay(outputAngles);

    // 获取顺序名称
    QString orderName;
    switch (order) {
        case HECEulerOrder::XYZ: orderName = "XYZ"; break;
        case HECEulerOrder::XZY: orderName = "XZY"; break;
        case HECEulerOrder::YXZ: orderName = "YXZ"; break;
        case HECEulerOrder::YZX: orderName = "YZX"; break;
        case HECEulerOrder::ZXY: orderName = "ZXY"; break;
        case HECEulerOrder::ZYX: orderName = "ZYX"; break;
    }

    appendLog(QString("欧拉角转换 (顺序: %1):").arg(orderName));

    double inRoll, inPitch, inYaw;
    inputAngles.toDegrees(inRoll, inPitch, inYaw);
    appendLog(QString("  输入: Roll=%1°, Pitch=%2°, Yaw=%3°")
        .arg(inRoll, 0, 'f', 2)
        .arg(inPitch, 0, 'f', 2)
        .arg(inYaw, 0, 'f', 2));

    double outRoll, outPitch, outYaw;
    outputAngles.toDegrees(outRoll, outPitch, outYaw);
    appendLog(QString("  输出: Roll=%1°, Pitch=%2°, Yaw=%3°")
        .arg(outRoll, 0, 'f', 2)
        .arg(outPitch, 0, 'f', 2)
        .arg(outYaw, 0, 'f', 2));
}

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

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

    // 重置欧拉角
    HECEulerAngles zeroAngles;
    updateEulerDisplay(zeroAngles);

    // 重置误差和质心
    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_logEdit->clear();

    m_hasResult = false;
}

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