GrabBag/Tools/RobotView/MainWindow.cpp

/**
 * @file MainWindow.cpp
 * @brief 机器人控制测试工具主窗口实现
 */

#include "MainWindow.h"
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QLabel>
#include <QLineEdit>
#include <QPushButton>
#include <QSpinBox>
#include <QDoubleSpinBox>
#include <QTextEdit>
#include <QTabWidget>
#include <QDateTime>
#include <QMessageBox>
#include <QSplitter>

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , robot_(std::make_unique<FairinoRobotWrapper>())
    , refreshTimer_(new QTimer(this))
{
    setupUI();
    setupConnections();

    // 显示SDK版本
    lblSDKVersion_->setText(QString("SDK版本: %1").arg(QString::fromStdString(robot_->GetSDKVersion())));

    updateConnectionState(false);
}

MainWindow::~MainWindow()
{
    if (robot_->IsConnected()) {
        robot_->Disconnect();
    }
}

void MainWindow::setupUI()
{
    setWindowTitle("RobotView - 机器人测试工具");
    setMinimumSize(620, 560);

    QWidget* centralWidget = new QWidget(this);
    setCentralWidget(centralWidget);

    QVBoxLayout* mainLayout = new QVBoxLayout(centralWidget);
    mainLayout->setSpacing(5);
    mainLayout->setContentsMargins(5, 5, 5, 5);

    // ========== 顶部连接控制区 ==========
    QGroupBox* groupConnection = new QGroupBox("连接控制", this);
    QVBoxLayout* connMainLayout = new QVBoxLayout(groupConnection);
    connMainLayout->setSpacing(3);
    connMainLayout->setContentsMargins(5, 5, 5, 5);

    // 第1行：IP + 连接/断开 + 状态
    QHBoxLayout* connRow1 = new QHBoxLayout();
    connRow1->setSpacing(3);
    connRow1->addWidget(new QLabel("IP:"));
    editIP_ = new QLineEdit("192.168.58.2");
    editIP_->setFixedWidth(120);
    connRow1->addWidget(editIP_);

    btnConnect_ = new QPushButton("连接");
    btnDisconnect_ = new QPushButton("断开");
    connRow1->addWidget(btnConnect_);
    connRow1->addWidget(btnDisconnect_);

    lblConnectionStatus_ = new QLabel("未连接");
    lblConnectionStatus_->setStyleSheet("color: red; font-weight: bold;");
    connRow1->addWidget(lblConnectionStatus_);
    connRow1->addStretch();
    connMainLayout->addLayout(connRow1);

    // 第2行：上下使能 + 清除错误 + SDK版本
    QHBoxLayout* connRow2 = new QHBoxLayout();
    connRow2->setSpacing(3);
    btnEnable_ = new QPushButton("上使能");
    btnDisable_ = new QPushButton("下使能");
    btnResetError_ = new QPushButton("清除错误");
    connRow2->addWidget(btnEnable_);
    connRow2->addWidget(btnDisable_);
    connRow2->addWidget(btnResetError_);
    connRow2->addStretch();
    lblSDKVersion_ = new QLabel("SDK版本: ");
    connRow2->addWidget(lblSDKVersion_);
    connMainLayout->addLayout(connRow2);

    mainLayout->addWidget(groupConnection);

    // ========== 中间区域：左侧Tab + 右侧状态 ==========
    QHBoxLayout* middleLayout = new QHBoxLayout();
    middleLayout->setSpacing(5);

    // --- 左侧 Tab 控件 ---
    tabWidget_ = new QTabWidget(this);

    // === Tab 1: 运动控制 ===
    QWidget* tabMotion = new QWidget();
    QVBoxLayout* motionLayout = new QVBoxLayout(tabMotion);
    motionLayout->setSpacing(3);

    // 运动参数区
    QGroupBox* groupParams = new QGroupBox("运动参数", tabMotion);
    QHBoxLayout* paramsLayout = new QHBoxLayout(groupParams);

    paramsLayout->addWidget(new QLabel("速度(%):"));
    spinVel_ = new QSpinBox();
    spinVel_->setRange(1, 100);
    spinVel_->setValue(30);
    paramsLayout->addWidget(spinVel_);

    paramsLayout->addWidget(new QLabel("加速度(%):"));
    spinAcc_ = new QSpinBox();
    spinAcc_->setRange(1, 100);
    spinAcc_->setValue(30);
    paramsLayout->addWidget(spinAcc_);

    paramsLayout->addWidget(new QLabel("工具号:"));
    spinTool_ = new QSpinBox();
    spinTool_->setRange(0, 14);
    paramsLayout->addWidget(spinTool_);

    paramsLayout->addWidget(new QLabel("工件号:"));
    spinUser_ = new QSpinBox();
    spinUser_->setRange(0, 14);
    paramsLayout->addWidget(spinUser_);

    paramsLayout->addStretch();
    motionLayout->addWidget(groupParams);

    // 位姿输入区（MoveL 和 MoveJ 共用）
    QGroupBox* groupPose = new QGroupBox("位姿运动 MoveL / MoveJ (mm/deg)", tabMotion);
    QGridLayout* poseLayout = new QGridLayout(groupPose);
    poseLayout->setHorizontalSpacing(2);
    poseLayout->setVerticalSpacing(3);
    poseLayout->setContentsMargins(5, 5, 5, 5);
    // 标签列不拉伸，SpinBox列均匀拉伸
    poseLayout->setColumnStretch(0, 0);
    poseLayout->setColumnStretch(1, 1);
    poseLayout->setColumnStretch(2, 0);
    poseLayout->setColumnStretch(3, 1);
    poseLayout->setColumnStretch(4, 0);
    poseLayout->setColumnStretch(5, 1);

    QLabel* lblX = new QLabel("X:");
    lblX->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblX, 0, 0);
    spinPoseX_ = new QDoubleSpinBox();
    spinPoseX_->setRange(-2000, 2000);
    spinPoseX_->setDecimals(2);
    spinPoseX_->setValue(400);
    poseLayout->addWidget(spinPoseX_, 0, 1);

    QLabel* lblY = new QLabel("Y:");
    lblY->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblY, 0, 2);
    spinPoseY_ = new QDoubleSpinBox();
    spinPoseY_->setRange(-2000, 2000);
    spinPoseY_->setDecimals(2);
    spinPoseY_->setValue(0);
    poseLayout->addWidget(spinPoseY_, 0, 3);

    QLabel* lblZ = new QLabel("Z:");
    lblZ->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblZ, 0, 4);
    spinPoseZ_ = new QDoubleSpinBox();
    spinPoseZ_->setRange(-2000, 2000);
    spinPoseZ_->setDecimals(2);
    spinPoseZ_->setValue(300);
    poseLayout->addWidget(spinPoseZ_, 0, 5);

    QLabel* lblRx = new QLabel("Rx:");
    lblRx->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblRx, 1, 0);
    spinPoseRx_ = new QDoubleSpinBox();
    spinPoseRx_->setRange(-180, 180);
    spinPoseRx_->setDecimals(2);
    spinPoseRx_->setValue(180);
    poseLayout->addWidget(spinPoseRx_, 1, 1);

    QLabel* lblRy = new QLabel("Ry:");
    lblRy->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblRy, 1, 2);
    spinPoseRy_ = new QDoubleSpinBox();
    spinPoseRy_->setRange(-180, 180);
    spinPoseRy_->setDecimals(2);
    spinPoseRy_->setValue(0);
    poseLayout->addWidget(spinPoseRy_, 1, 3);

    QLabel* lblRz = new QLabel("Rz:");
    lblRz->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    poseLayout->addWidget(lblRz, 1, 4);
    spinPoseRz_ = new QDoubleSpinBox();
    spinPoseRz_->setRange(-180, 180);
    spinPoseRz_->setDecimals(2);
    spinPoseRz_->setValue(0);
    poseLayout->addWidget(spinPoseRz_, 1, 5);

    // 按钮行
    QHBoxLayout* poseBtnLayout = new QHBoxLayout();
    btnMoveL_ = new QPushButton("MoveL 直线");
    btnMoveJPose_ = new QPushButton("MoveJ 关节");
    btnSyncPose_ = new QPushButton("同步当前位姿");
    poseBtnLayout->addWidget(btnMoveL_);
    poseBtnLayout->addWidget(btnMoveJPose_);
    poseBtnLayout->addWidget(btnSyncPose_);
    poseBtnLayout->addStretch();
    poseLayout->addLayout(poseBtnLayout, 2, 0, 1, 6);

    motionLayout->addWidget(groupPose);

    // 关节角度输入区
    QGroupBox* groupJoint = new QGroupBox("关节运动 MoveJ (deg)", tabMotion);
    QGridLayout* jointLayout = new QGridLayout(groupJoint);
    jointLayout->setHorizontalSpacing(2);
    jointLayout->setVerticalSpacing(3);
    jointLayout->setContentsMargins(5, 5, 5, 5);
    jointLayout->setColumnStretch(0, 0);
    jointLayout->setColumnStretch(1, 1);
    jointLayout->setColumnStretch(2, 0);
    jointLayout->setColumnStretch(3, 1);
    jointLayout->setColumnStretch(4, 0);
    jointLayout->setColumnStretch(5, 1);

    QLabel* lblJ1 = new QLabel("J1:");
    lblJ1->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ1, 0, 0);
    spinJoint1_ = new QDoubleSpinBox();
    spinJoint1_->setRange(-180, 180);
    spinJoint1_->setDecimals(2);
    jointLayout->addWidget(spinJoint1_, 0, 1);

    QLabel* lblJ2 = new QLabel("J2:");
    lblJ2->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ2, 0, 2);
    spinJoint2_ = new QDoubleSpinBox();
    spinJoint2_->setRange(-180, 180);
    spinJoint2_->setDecimals(2);
    spinJoint2_->setValue(-45);
    jointLayout->addWidget(spinJoint2_, 0, 3);

    QLabel* lblJ3 = new QLabel("J3:");
    lblJ3->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ3, 0, 4);
    spinJoint3_ = new QDoubleSpinBox();
    spinJoint3_->setRange(-180, 180);
    spinJoint3_->setDecimals(2);
    spinJoint3_->setValue(90);
    jointLayout->addWidget(spinJoint3_, 0, 5);

    QLabel* lblJ4 = new QLabel("J4:");
    lblJ4->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ4, 1, 0);
    spinJoint4_ = new QDoubleSpinBox();
    spinJoint4_->setRange(-180, 180);
    spinJoint4_->setDecimals(2);
    jointLayout->addWidget(spinJoint4_, 1, 1);

    QLabel* lblJ5 = new QLabel("J5:");
    lblJ5->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ5, 1, 2);
    spinJoint5_ = new QDoubleSpinBox();
    spinJoint5_->setRange(-180, 180);
    spinJoint5_->setDecimals(2);
    spinJoint5_->setValue(45);
    jointLayout->addWidget(spinJoint5_, 1, 3);

    QLabel* lblJ6 = new QLabel("J6:");
    lblJ6->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    jointLayout->addWidget(lblJ6, 1, 4);
    spinJoint6_ = new QDoubleSpinBox();
    spinJoint6_->setRange(-180, 180);
    spinJoint6_->setDecimals(2);
    jointLayout->addWidget(spinJoint6_, 1, 5);

    // 按钮行
    QHBoxLayout* jointBtnLayout = new QHBoxLayout();
    btnMoveJJoint_ = new QPushButton("MoveJ 执行");
    btnSyncJoint_ = new QPushButton("同步当前关节");
    jointBtnLayout->addWidget(btnMoveJJoint_);
    jointBtnLayout->addWidget(btnSyncJoint_);
    jointBtnLayout->addStretch();
    jointLayout->addLayout(jointBtnLayout, 2, 0, 1, 6);

    motionLayout->addWidget(groupJoint);

    // 运动控制按钮
    QGroupBox* groupControl = new QGroupBox("运动控制", tabMotion);
    QHBoxLayout* controlLayout = new QHBoxLayout(groupControl);
    controlLayout->setSpacing(3);
    controlLayout->setContentsMargins(5, 5, 5, 5);

    btnStop_ = new QPushButton("停止运动");
    btnPause_ = new QPushButton("暂停运动");
    btnResume_ = new QPushButton("恢复运动");

    controlLayout->addWidget(btnStop_);
    controlLayout->addWidget(btnPause_);
    controlLayout->addWidget(btnResume_);
    controlLayout->addStretch();

    motionLayout->addWidget(groupControl);

    tabWidget_->addTab(tabMotion, "运动控制");

    // === Tab 2: 运动学 ===
    QWidget* tabKin = new QWidget();
    QVBoxLayout* kinLayout = new QVBoxLayout(tabKin);

    // 正运动学
    QGroupBox* groupFK = new QGroupBox("正运动学 (关节角度 -> TCP位姿)", tabKin);
    QGridLayout* fkLayout = new QGridLayout(groupFK);
    fkLayout->setHorizontalSpacing(5);

    QLabel* lblFKJ1 = new QLabel("J1:");
    lblFKJ1->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ1, 0, 0);
    spinFKJ1_ = new QDoubleSpinBox();
    spinFKJ1_->setRange(-180, 180);
    spinFKJ1_->setDecimals(2);
    fkLayout->addWidget(spinFKJ1_, 0, 1);

    QLabel* lblFKJ2 = new QLabel("J2:");
    lblFKJ2->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ2, 0, 2);
    spinFKJ2_ = new QDoubleSpinBox();
    spinFKJ2_->setRange(-180, 180);
    spinFKJ2_->setDecimals(2);
    spinFKJ2_->setValue(-45);
    fkLayout->addWidget(spinFKJ2_, 0, 3);

    QLabel* lblFKJ3 = new QLabel("J3:");
    lblFKJ3->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ3, 0, 4);
    spinFKJ3_ = new QDoubleSpinBox();
    spinFKJ3_->setRange(-180, 180);
    spinFKJ3_->setDecimals(2);
    spinFKJ3_->setValue(90);
    fkLayout->addWidget(spinFKJ3_, 0, 5);

    QLabel* lblFKJ4 = new QLabel("J4:");
    lblFKJ4->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ4, 1, 0);
    spinFKJ4_ = new QDoubleSpinBox();
    spinFKJ4_->setRange(-180, 180);
    spinFKJ4_->setDecimals(2);
    fkLayout->addWidget(spinFKJ4_, 1, 1);

    QLabel* lblFKJ5 = new QLabel("J5:");
    lblFKJ5->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ5, 1, 2);
    spinFKJ5_ = new QDoubleSpinBox();
    spinFKJ5_->setRange(-180, 180);
    spinFKJ5_->setDecimals(2);
    spinFKJ5_->setValue(45);
    fkLayout->addWidget(spinFKJ5_, 1, 3);

    QLabel* lblFKJ6 = new QLabel("J6:");
    lblFKJ6->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    fkLayout->addWidget(lblFKJ6, 1, 4);
    spinFKJ6_ = new QDoubleSpinBox();
    spinFKJ6_->setRange(-180, 180);
    spinFKJ6_->setDecimals(2);
    fkLayout->addWidget(spinFKJ6_, 1, 5);

    btnForwardKin_ = new QPushButton("计算");
    fkLayout->addWidget(btnForwardKin_, 0, 6, 2, 1);

    fkLayout->addWidget(new QLabel("结果:"), 2, 0);
    lblFKResult_ = new QLabel("--");
    lblFKResult_->setWordWrap(true);
    fkLayout->addWidget(lblFKResult_, 2, 1, 1, 6);

    kinLayout->addWidget(groupFK);

    // 逆运动学
    QGroupBox* groupIK = new QGroupBox("逆运动学 (TCP位姿 -> 关节角度)", tabKin);
    QGridLayout* ikLayout = new QGridLayout(groupIK);
    ikLayout->setHorizontalSpacing(5);

    QLabel* lblIKX = new QLabel("X:");
    lblIKX->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKX, 0, 0);
    spinIKX_ = new QDoubleSpinBox();
    spinIKX_->setRange(-2000, 2000);
    spinIKX_->setDecimals(2);
    spinIKX_->setValue(400);
    ikLayout->addWidget(spinIKX_, 0, 1);

    QLabel* lblIKY = new QLabel("Y:");
    lblIKY->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKY, 0, 2);
    spinIKY_ = new QDoubleSpinBox();
    spinIKY_->setRange(-2000, 2000);
    spinIKY_->setDecimals(2);
    ikLayout->addWidget(spinIKY_, 0, 3);

    QLabel* lblIKZ = new QLabel("Z:");
    lblIKZ->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKZ, 0, 4);
    spinIKZ_ = new QDoubleSpinBox();
    spinIKZ_->setRange(-2000, 2000);
    spinIKZ_->setDecimals(2);
    spinIKZ_->setValue(300);
    ikLayout->addWidget(spinIKZ_, 0, 5);

    QLabel* lblIKRx = new QLabel("Rx:");
    lblIKRx->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKRx, 1, 0);
    spinIKRx_ = new QDoubleSpinBox();
    spinIKRx_->setRange(-180, 180);
    spinIKRx_->setDecimals(2);
    spinIKRx_->setValue(180);
    ikLayout->addWidget(spinIKRx_, 1, 1);

    QLabel* lblIKRy = new QLabel("Ry:");
    lblIKRy->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKRy, 1, 2);
    spinIKRy_ = new QDoubleSpinBox();
    spinIKRy_->setRange(-180, 180);
    spinIKRy_->setDecimals(2);
    ikLayout->addWidget(spinIKRy_, 1, 3);

    QLabel* lblIKRz = new QLabel("Rz:");
    lblIKRz->setAlignment(Qt::AlignRight | Qt::AlignVCenter);
    ikLayout->addWidget(lblIKRz, 1, 4);
    spinIKRz_ = new QDoubleSpinBox();
    spinIKRz_->setRange(-180, 180);
    spinIKRz_->setDecimals(2);
    ikLayout->addWidget(spinIKRz_, 1, 5);

    btnInverseKin_ = new QPushButton("计算");
    ikLayout->addWidget(btnInverseKin_, 0, 6, 2, 1);

    ikLayout->addWidget(new QLabel("结果:"), 2, 0);
    lblIKResult_ = new QLabel("--");
    lblIKResult_->setWordWrap(true);
    ikLayout->addWidget(lblIKResult_, 2, 1, 1, 6);

    kinLayout->addWidget(groupIK);

    tabWidget_->addTab(tabKin, "运动学");

    middleLayout->addWidget(tabWidget_, 2);

    // --- 右侧状态显示区（始终可见） ---
    QWidget* statusWidget = new QWidget();
    QVBoxLayout* statusMainLayout = new QVBoxLayout(statusWidget);
    statusMainLayout->setContentsMargins(5, 0, 0, 0);
    statusMainLayout->setSpacing(5);

    // 关节状态
    QGroupBox* groupJoints = new QGroupBox("关节位置 (deg)");
    QGridLayout* jointsLayout = new QGridLayout(groupJoints);

    jointsLayout->addWidget(new QLabel("J1:"), 0, 0);
    lblJoint1_ = new QLabel("--");
    lblJoint1_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint1_, 0, 1);

    jointsLayout->addWidget(new QLabel("J2:"), 1, 0);
    lblJoint2_ = new QLabel("--");
    lblJoint2_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint2_, 1, 1);

    jointsLayout->addWidget(new QLabel("J3:"), 2, 0);
    lblJoint3_ = new QLabel("--");
    lblJoint3_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint3_, 2, 1);

    jointsLayout->addWidget(new QLabel("J4:"), 3, 0);
    lblJoint4_ = new QLabel("--");
    lblJoint4_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint4_, 3, 1);

    jointsLayout->addWidget(new QLabel("J5:"), 4, 0);
    lblJoint5_ = new QLabel("--");
    lblJoint5_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint5_, 4, 1);

    jointsLayout->addWidget(new QLabel("J6:"), 5, 0);
    lblJoint6_ = new QLabel("--");
    lblJoint6_->setStyleSheet("font-weight: bold; color: #2196F3; min-width: 50px;");
    jointsLayout->addWidget(lblJoint6_, 5, 1);

    statusMainLayout->addWidget(groupJoints);

    // TCP 位姿
    QGroupBox* groupTcp = new QGroupBox("TCP位姿 (mm/deg)");
    QGridLayout* tcpLayout = new QGridLayout(groupTcp);

    tcpLayout->addWidget(new QLabel("X:"), 0, 0);
    lblTcpX_ = new QLabel("--");
    lblTcpX_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpX_, 0, 1);

    tcpLayout->addWidget(new QLabel("Y:"), 1, 0);
    lblTcpY_ = new QLabel("--");
    lblTcpY_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpY_, 1, 1);

    tcpLayout->addWidget(new QLabel("Z:"), 2, 0);
    lblTcpZ_ = new QLabel("--");
    lblTcpZ_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpZ_, 2, 1);

    tcpLayout->addWidget(new QLabel("Rx:"), 3, 0);
    lblTcpRx_ = new QLabel("--");
    lblTcpRx_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpRx_, 3, 1);

    tcpLayout->addWidget(new QLabel("Ry:"), 4, 0);
    lblTcpRy_ = new QLabel("--");
    lblTcpRy_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpRy_, 4, 1);

    tcpLayout->addWidget(new QLabel("Rz:"), 5, 0);
    lblTcpRz_ = new QLabel("--");
    lblTcpRz_->setStyleSheet("font-weight: bold; color: #4CAF50; min-width: 50px;");
    tcpLayout->addWidget(lblTcpRz_, 5, 1);

    statusMainLayout->addWidget(groupTcp);

    // 机器人状态
    QGroupBox* groupRobotState = new QGroupBox("机器人状态");
    QGridLayout* robotStateLayout = new QGridLayout(groupRobotState);

    robotStateLayout->addWidget(new QLabel("状态:"), 0, 0);
    lblRobotState_ = new QLabel("--");
    robotStateLayout->addWidget(lblRobotState_, 0, 1);

    robotStateLayout->addWidget(new QLabel("到位:"), 1, 0);
    lblMotionDone_ = new QLabel("--");
    robotStateLayout->addWidget(lblMotionDone_, 1, 1);

    robotStateLayout->addWidget(new QLabel("错误:"), 2, 0);
    lblErrorCode_ = new QLabel("--");
    robotStateLayout->addWidget(lblErrorCode_, 2, 1);

    statusMainLayout->addWidget(groupRobotState);

    // 刷新控制
    QHBoxLayout* refreshLayout = new QHBoxLayout();
    btnRefreshState_ = new QPushButton("刷新");
    btnAutoRefresh_ = new QPushButton("自动");
    btnAutoRefresh_->setCheckable(true);
    refreshLayout->addWidget(btnRefreshState_);
    refreshLayout->addWidget(btnAutoRefresh_);
    statusMainLayout->addLayout(refreshLayout);

    statusWidget->setFixedWidth(140);
    middleLayout->addWidget(statusWidget);

    mainLayout->addLayout(middleLayout, 1);

    // ========== 底部日志区 ==========
    QGroupBox* groupLog = new QGroupBox("日志", this);
    QVBoxLayout* logLayout = new QVBoxLayout(groupLog);
    textLog_ = new QTextEdit();
    textLog_->setReadOnly(true);
    textLog_->setMaximumHeight(80);
    logLayout->addWidget(textLog_);
    mainLayout->addWidget(groupLog);
}

void MainWindow::setupConnections()
{
    // 连接控制
    connect(btnConnect_, &QPushButton::clicked, this, &MainWindow::onConnectClicked);
    connect(btnDisconnect_, &QPushButton::clicked, this, &MainWindow::onDisconnectClicked);
    connect(btnEnable_, &QPushButton::clicked, this, &MainWindow::onEnableClicked);
    connect(btnDisable_, &QPushButton::clicked, this, &MainWindow::onDisableClicked);
    connect(btnResetError_, &QPushButton::clicked, this, &MainWindow::onResetErrorClicked);

    // 运动控制
    connect(btnMoveL_, &QPushButton::clicked, this, &MainWindow::onMoveLClicked);
    connect(btnMoveJPose_, &QPushButton::clicked, this, &MainWindow::onMoveJPoseClicked);
    connect(btnMoveJJoint_, &QPushButton::clicked, this, &MainWindow::onMoveJJointClicked);
    connect(btnStop_, &QPushButton::clicked, this, &MainWindow::onStopClicked);
    connect(btnPause_, &QPushButton::clicked, this, &MainWindow::onPauseClicked);
    connect(btnResume_, &QPushButton::clicked, this, &MainWindow::onResumeClicked);

    // 同步按钮
    connect(btnSyncPose_, &QPushButton::clicked, this, &MainWindow::onSyncPoseClicked);
    connect(btnSyncJoint_, &QPushButton::clicked, this, &MainWindow::onSyncJointClicked);

    // 状态刷新
    connect(btnRefreshState_, &QPushButton::clicked, this, &MainWindow::onRefreshStateClicked);
    connect(btnAutoRefresh_, &QPushButton::toggled, this, &MainWindow::onAutoRefreshToggled);
    connect(refreshTimer_, &QTimer::timeout, this, &MainWindow::updateRobotState);

    // 运动学
    connect(btnForwardKin_, &QPushButton::clicked, this, &MainWindow::onForwardKinClicked);
    connect(btnInverseKin_, &QPushButton::clicked, this, &MainWindow::onInverseKinClicked);
}

void MainWindow::log(const QString& msg)
{
    QString timestamp = QDateTime::currentDateTime().toString("hh:mm:ss.zzz");
    textLog_->append(QString("[%1] %2").arg(timestamp, msg));
}

void MainWindow::updateConnectionState(bool connected)
{
    if (connected) {
        lblConnectionStatus_->setText("已连接");
        lblConnectionStatus_->setStyleSheet("color: green; font-weight: bold;");
    } else {
        lblConnectionStatus_->setText("未连接");
        lblConnectionStatus_->setStyleSheet("color: red; font-weight: bold;");
    }

    btnConnect_->setEnabled(!connected);
    btnDisconnect_->setEnabled(connected);
    btnEnable_->setEnabled(connected);
    btnDisable_->setEnabled(connected);
    btnResetError_->setEnabled(connected);
    btnMoveL_->setEnabled(connected);
    btnMoveJPose_->setEnabled(connected);
    btnMoveJJoint_->setEnabled(connected);
    btnSyncPose_->setEnabled(connected);
    btnSyncJoint_->setEnabled(connected);
    btnStop_->setEnabled(connected);
    btnPause_->setEnabled(connected);
    btnResume_->setEnabled(connected);
    btnRefreshState_->setEnabled(connected);
    btnAutoRefresh_->setEnabled(connected);
    btnForwardKin_->setEnabled(connected);
    btnInverseKin_->setEnabled(connected);
}

// ========== 连接控制槽函数 ==========

void MainWindow::onConnectClicked()
{
    QString ip = editIP_->text().trimmed();
    log(QString("正在连接机器人: %1").arg(ip));

    errno_t ret = robot_->Connect(ip.toStdString());
    if (ret == ERR_SUCCESS) {
        log("连接成功");
        updateConnectionState(true);
        updateRobotState();
    } else {
        log(QString("连接失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
        updateConnectionState(false);
    }
}

void MainWindow::onDisconnectClicked()
{
    log("正在断开连接...");
    robot_->Disconnect();
    log("已断开连接");
    updateConnectionState(false);
    btnAutoRefresh_->setChecked(false);
}

void MainWindow::onEnableClicked()
{
    errno_t ret = robot_->RobotEnable(1);
    if (ret == ERR_SUCCESS) {
        log("机器人已上使能");
    } else {
        log(QString("上使能失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onDisableClicked()
{
    errno_t ret = robot_->RobotEnable(0);
    if (ret == ERR_SUCCESS) {
        log("机器人已下使能");
    } else {
        log(QString("下使能失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onResetErrorClicked()
{
    errno_t ret = robot_->ResetAllError();
    if (ret == ERR_SUCCESS) {
        log("错误已清除");
    } else {
        log(QString("清除错误失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

// ========== 运动控制槽函数 ==========

void MainWindow::onMoveLClicked()
{
    double x = spinPoseX_->value();
    double y = spinPoseY_->value();
    double z = spinPoseZ_->value();
    double rx = spinPoseRx_->value();
    double ry = spinPoseRy_->value();
    double rz = spinPoseRz_->value();
    float vel = spinVel_->value();
    float acc = spinAcc_->value();
    int tool = spinTool_->value();
    int user = spinUser_->value();

    log(QString("MoveL: X=%1, Y=%2, Z=%3, Rx=%4, Ry=%5, Rz=%6").arg(x).arg(y).arg(z).arg(rx).arg(ry).arg(rz));

    errno_t ret = robot_->MoveL(x, y, z, rx, ry, rz, vel, acc, -1.0f, tool, user);
    if (ret == ERR_SUCCESS) {
        log("MoveL 指令已发送");
    } else {
        log(QString("MoveL 失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onMoveJPoseClicked()
{
    double x = spinPoseX_->value();
    double y = spinPoseY_->value();
    double z = spinPoseZ_->value();
    double rx = spinPoseRx_->value();
    double ry = spinPoseRy_->value();
    double rz = spinPoseRz_->value();
    float vel = spinVel_->value();
    float acc = spinAcc_->value();
    int tool = spinTool_->value();
    int user = spinUser_->value();

    log(QString("MoveJ(Pose): X=%1, Y=%2, Z=%3, Rx=%4, Ry=%5, Rz=%6").arg(x).arg(y).arg(z).arg(rx).arg(ry).arg(rz));

    errno_t ret = robot_->MoveJByPose(x, y, z, rx, ry, rz, vel, acc, -1.0f, tool, user);
    if (ret == ERR_SUCCESS) {
        log("MoveJ(Pose) 指令已发送");
    } else {
        log(QString("MoveJ(Pose) 失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onMoveJJointClicked()
{
    double j1 = spinJoint1_->value();
    double j2 = spinJoint2_->value();
    double j3 = spinJoint3_->value();
    double j4 = spinJoint4_->value();
    double j5 = spinJoint5_->value();
    double j6 = spinJoint6_->value();
    float vel = spinVel_->value();
    float acc = spinAcc_->value();
    int tool = spinTool_->value();
    int user = spinUser_->value();

    log(QString("MoveJ(Joint): J1=%1, J2=%2, J3=%3, J4=%4, J5=%5, J6=%6").arg(j1).arg(j2).arg(j3).arg(j4).arg(j5).arg(j6));

    errno_t ret = robot_->MoveJByJoint(j1, j2, j3, j4, j5, j6, vel, acc, -1.0f, tool, user);
    if (ret == ERR_SUCCESS) {
        log("MoveJ(Joint) 指令已发送");
    } else {
        log(QString("MoveJ(Joint) 失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onStopClicked()
{
    errno_t ret = robot_->StopMotion();
    if (ret == ERR_SUCCESS) {
        log("运动已停止");
    } else {
        log(QString("停止失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onPauseClicked()
{
    errno_t ret = robot_->PauseMotion();
    if (ret == ERR_SUCCESS) {
        log("运动已暂停");
    } else {
        log(QString("暂停失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onResumeClicked()
{
    errno_t ret = robot_->ResumeMotion();
    if (ret == ERR_SUCCESS) {
        log("运动已恢复");
    } else {
        log(QString("恢复失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

// ========== 同步位置槽函数 ==========

void MainWindow::onSyncPoseClicked()
{
    double x, y, z, rx, ry, rz;
    if (robot_->GetCurrentTCPPose(x, y, z, rx, ry, rz) == ERR_SUCCESS) {
        spinPoseX_->setValue(x);
        spinPoseY_->setValue(y);
        spinPoseZ_->setValue(z);
        spinPoseRx_->setValue(rx);
        spinPoseRy_->setValue(ry);
        spinPoseRz_->setValue(rz);
        log("已同步当前位姿到输入框");
    } else {
        log(QString("获取位姿失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onSyncJointClicked()
{
    double j1, j2, j3, j4, j5, j6;
    if (robot_->GetCurrentJointPos(j1, j2, j3, j4, j5, j6) == ERR_SUCCESS) {
        spinJoint1_->setValue(j1);
        spinJoint2_->setValue(j2);
        spinJoint3_->setValue(j3);
        spinJoint4_->setValue(j4);
        spinJoint5_->setValue(j5);
        spinJoint6_->setValue(j6);
        log("已同步当前关节到输入框");
    } else {
        log(QString("获取关节失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

// ========== 状态刷新槽函数 ==========

void MainWindow::onRefreshStateClicked()
{
    updateRobotState();
}

void MainWindow::onAutoRefreshToggled(bool checked)
{
    if (checked) {
        refreshTimer_->start(200);  // 200ms 刷新一次
        log("自动刷新已开启");
    } else {
        refreshTimer_->stop();
        log("自动刷新已关闭");
    }
}

void MainWindow::updateRobotState()
{
    if (!robot_->IsConnected()) return;

    // 获取关节位置
    double j1, j2, j3, j4, j5, j6;
    if (robot_->GetCurrentJointPos(j1, j2, j3, j4, j5, j6) == ERR_SUCCESS) {
        lblJoint1_->setText(QString::number(j1, 'f', 2));
        lblJoint2_->setText(QString::number(j2, 'f', 2));
        lblJoint3_->setText(QString::number(j3, 'f', 2));
        lblJoint4_->setText(QString::number(j4, 'f', 2));
        lblJoint5_->setText(QString::number(j5, 'f', 2));
        lblJoint6_->setText(QString::number(j6, 'f', 2));
    }

    // 获取TCP位姿
    double x, y, z, rx, ry, rz;
    if (robot_->GetCurrentTCPPose(x, y, z, rx, ry, rz) == ERR_SUCCESS) {
        lblTcpX_->setText(QString::number(x, 'f', 2));
        lblTcpY_->setText(QString::number(y, 'f', 2));
        lblTcpZ_->setText(QString::number(z, 'f', 2));
        lblTcpRx_->setText(QString::number(rx, 'f', 2));
        lblTcpRy_->setText(QString::number(ry, 'f', 2));
        lblTcpRz_->setText(QString::number(rz, 'f', 2));
        emit tcpPoseUpdated(x, y, z, rx, ry, rz);
    }

    // 获取运动状态
    uint8_t motionDone;
    if (robot_->GetMotionDone(motionDone) == ERR_SUCCESS) {
        lblMotionDone_->setText(motionDone ? "是" : "否");
    }

    // 获取错误码
    int maincode, subcode;
    if (robot_->GetErrorCode(maincode, subcode) == ERR_SUCCESS) {
        lblErrorCode_->setText(QString("%1/%2").arg(maincode).arg(subcode));
        if (maincode != 0) {
            lblErrorCode_->setStyleSheet("color: red; font-weight: bold;");
        } else {
            lblErrorCode_->setStyleSheet("color: green;");
        }
    }

    // 获取机器人状态
    ROBOT_STATE_PKG state;
    if (robot_->GetRobotState(state) == ERR_SUCCESS) {
        QString stateStr;
        switch (state.robot_state) {
        case 1: stateStr = "停止"; break;
        case 2: stateStr = "运行"; break;
        case 3: stateStr = "暂停"; break;
        case 4: stateStr = "拖动"; break;
        default: stateStr = QString("未知(%1)").arg(state.robot_state);
        }
        lblRobotState_->setText(stateStr);
    }
}

// ========== 运动学槽函数 ==========

void MainWindow::onForwardKinClicked()
{
    double j1 = spinFKJ1_->value();
    double j2 = spinFKJ2_->value();
    double j3 = spinFKJ3_->value();
    double j4 = spinFKJ4_->value();
    double j5 = spinFKJ5_->value();
    double j6 = spinFKJ6_->value();

    double x, y, z, rx, ry, rz;
    errno_t ret = robot_->ForwardKinematics(j1, j2, j3, j4, j5, j6, x, y, z, rx, ry, rz);

    if (ret == ERR_SUCCESS) {
        lblFKResult_->setText(QString("X=%1, Y=%2, Z=%3, Rx=%4, Ry=%5, Rz=%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));
        log("正运动学计算完成");
    } else {
        lblFKResult_->setText(QString("计算失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}

void MainWindow::onInverseKinClicked()
{
    double x = spinIKX_->value();
    double y = spinIKY_->value();
    double z = spinIKZ_->value();
    double rx = spinIKRx_->value();
    double ry = spinIKRy_->value();
    double rz = spinIKRz_->value();

    double j1, j2, j3, j4, j5, j6;
    errno_t ret = robot_->InverseKinematics(x, y, z, rx, ry, rz, j1, j2, j3, j4, j5, j6);

    if (ret == ERR_SUCCESS) {
        lblIKResult_->setText(QString("J1=%1, J2=%2, J3=%3, J4=%4, J5=%5, J6=%6")
                              .arg(j1, 0, 'f', 2).arg(j2, 0, 'f', 2).arg(j3, 0, 'f', 2)
                              .arg(j4, 0, 'f', 2).arg(j5, 0, 'f', 2).arg(j6, 0, 'f', 2));
        log("逆运动学计算完成");
    } else {
        lblIKResult_->setText(QString("计算失败: %1").arg(QString::fromStdString(robot_->GetLastErrorString())));
    }
}