GrabBag/Tools/ControlApp/DemoControlPresenter.cpp

#include "DemoControlPresenter.h"
#include <QDebug>
#include <chrono>
#include <thread>

DemoControlPresenter::DemoControlPresenter(QObject *parent)
    : QObject(parent)
{
    m_robot = std::make_unique<FairinoRobotWrapper>();
}

DemoControlPresenter::~DemoControlPresenter()
{
    StopWorkflow();
    DisconnectController();
    DisconnectRobot();
}

int DemoControlPresenter::Init()
{
    SendStatusMessage("主控程序初始化完成");
    return 0;
}

bool DemoControlPresenter::ConnectToController(const QString& ip, int port)
{
    if (m_controllerConnected) {
        SendStatusMessage("控制器已连接");
        return true;
    }

    SendStatusMessage(QString("正在连接控制器 %1:%2...").arg(ip).arg(port));

    // 创建ModbusTCP客户端
    if (!IYModbusTCPClient::CreateInstance(&m_modbusClient, ip.toStdString(), port)) {
        SendStatusMessage("创建ModbusTCP客户端失败");
        emit controllerConnectionChanged(false);
        return false;
    }

    // 设置连接状态回调
    m_modbusClient->SetConnectionStateCallback([this](IYModbusTCPClient::ConnectionState oldState,
                                                       IYModbusTCPClient::ConnectionState newState,
                                                       const std::string& message) {
        QString msg = QString::fromStdString(message);
        SendStatusMessage(QString("控制器连接状态: %1").arg(msg));

        bool connected = (newState == IYModbusTCPClient::CONNECTED);
        m_controllerConnected = connected;
        emit controllerConnectionChanged(connected);
    });

    // 连接
    auto result = m_modbusClient->Connect();
    if (result != IYModbusTCPClient::SUCCESS) {
        SendStatusMessage(QString("连接控制器失败: %1")
                         .arg(QString::fromStdString(m_modbusClient->GetLastError())));
        emit controllerConnectionChanged(false);
        return false;
    }

    m_controllerConnected = true;
    SendStatusMessage("控制器连接成功");
    emit controllerConnectionChanged(true);
    return true;
}

void DemoControlPresenter::DisconnectController()
{
    if (m_modbusClient) {
        m_modbusClient->Disconnect();
        delete m_modbusClient;
        m_modbusClient = nullptr;
    }
    m_controllerConnected = false;
    emit controllerConnectionChanged(false);
    SendStatusMessage("控制器已断开连接");
}

bool DemoControlPresenter::ConnectToRobot(const QString& ip)
{
    if (m_robotConnected) {
        SendStatusMessage("机械臂已连接");
        return true;
    }

    SendStatusMessage(QString("正在连接机械臂 %1...").arg(ip));

    errno_t ret = m_robot->Connect(ip.toStdString());
    if (ret != 0) {
        SendStatusMessage(QString("连接机械臂失败: 错误码 %1").arg(ret));
        emit robotConnectionChanged(false);
        return false;
    }

    m_robotConnected = true;
    SendStatusMessage("机械臂连接成功");
    emit robotConnectionChanged(true);
    return true;
}

void DemoControlPresenter::DisconnectRobot()
{
    if (m_robot) {
        m_robot->Disconnect();
    }
    m_robotConnected = false;
    emit robotConnectionChanged(false);
    SendStatusMessage("机械臂已断开连接");
}

bool DemoControlPresenter::IsControllerConnected() const
{
    return m_controllerConnected;
}

bool DemoControlPresenter::IsRobotConnected() const
{
    return m_robotConnected;
}

void DemoControlPresenter::StartWorkflow()
{
    if (m_workflowRunning) {
        SendStatusMessage("工作流程已在运行中");
        return;
    }

    if (!m_controllerConnected) {
        SendStatusMessage("错误: 控制器未连接");
        return;
    }

    if (!m_robotConnected) {
        SendStatusMessage("错误: 机械臂未连接");
        return;
    }

    // 清理旧的线程对象（如果存在）
    if (m_workflowThread.joinable()) {
        m_workflowThread.join();
    }

    m_workflowShouldStop = false;
    m_workflowPaused = false;
    m_workflowStepMode = false;
    m_nextStepToExecute = 0;  // 从第一步开始
    m_workflowRunning = true;

    // 启动工作流程线程
    m_workflowThread = std::thread(&DemoControlPresenter::WorkflowThreadFunc, this);

    SendStatusMessage("工作流程已启动");
}

void DemoControlPresenter::StopWorkflow()
{
    if (!m_workflowRunning) {
        return;
    }

    m_workflowShouldStop = true;
    m_workflowPaused = false;
    m_workflowStepMode = false;
    m_nextStepToExecute = 0;  // 重置步骤

    // 唤醒可能在等待的线程
    m_stepCondition.notify_all();

    if (m_workflowThread.joinable()) {
        m_workflowThread.join();
    }

    m_workflowRunning = false;
    UpdateWorkflowStep(WorkflowStep::Idle, "工作流程已停止");
}

void DemoControlPresenter::PauseWorkflow()
{
    if (!m_workflowRunning || m_workflowPaused) {
        return;
    }

    m_workflowPaused = true;
    SendStatusMessage("工作流程已暂停");
}

void DemoControlPresenter::ResumeWorkflow()
{
    if (!m_workflowRunning || !m_workflowPaused) {
        return;
    }

    m_workflowPaused = false;
    m_workflowStepMode = false;
    m_stepCondition.notify_all();
    SendStatusMessage("工作流程已恢复");
}

void DemoControlPresenter::StepWorkflow()
{
    if (m_workflowRunning && !m_workflowPaused) {
        SendStatusMessage("请先暂停工作流程");
        return;
    }

    if (!m_controllerConnected) {
        SendStatusMessage("错误: 控制器未连接");
        return;
    }

    if (!m_robotConnected) {
        SendStatusMessage("错误: 机械臂未连接");
        return;
    }

    // 如果工作流程未运行，启动单步模式
    if (!m_workflowRunning) {
        // 清理旧的线程对象（如果存在）
        if (m_workflowThread.joinable()) {
            m_workflowThread.join();
        }

        m_workflowShouldStop = false;
        m_workflowPaused = true;
        m_workflowStepMode = true;
        m_workflowStepReady = true;
        m_nextStepToExecute = 0;  // 从第一步开始
        m_workflowRunning = true;

        // 启动工作流程线程
        m_workflowThread = std::thread(&DemoControlPresenter::WorkflowThreadFunc, this);
        SendStatusMessage("单步执行已启动，从第一步开始");
    } else {
        // 工作流程已运行且暂停，切换到单步模式并执行下一步
        m_workflowStepMode = true;  // 切换到单步模式
        m_workflowStepReady = true;
        m_stepCondition.notify_all();
        SendStatusMessage(QString("单步执行下一步（步骤 %1）").arg(m_nextStepToExecute + 1));
    }
}

void DemoControlPresenter::SetReadyPosition(double x, double y, double z, double rx, double ry, double rz)
{
    m_readyPosition.x = x;
    m_readyPosition.y = y;
    m_readyPosition.z = z;
    m_readyPosition.rx = rx;
    m_readyPosition.ry = ry;
    m_readyPosition.rz = rz;
}

void DemoControlPresenter::SetPlacePosition(double x, double y, double z, double rx, double ry, double rz)
{
    m_placePosition.x = x;
    m_placePosition.y = y;
    m_placePosition.z = z;
    m_placePosition.rx = rx;
    m_placePosition.ry = ry;
    m_placePosition.rz = rz;
}

void DemoControlPresenter::SetGraspOffset(double offsetZ)
{
    m_graspOffsetZ = offsetZ;
}

void DemoControlPresenter::SetToolNumber(int tool)
{
    m_toolNumber = tool;
}

void DemoControlPresenter::SetVelocity(double velocity)
{
    m_velocity = velocity;
}

void DemoControlPresenter::SetAcceleration(double acceleration)
{
    m_acceleration = acceleration;
}

bool DemoControlPresenter::TestMoveToReady()
{
    if (!m_robotConnected) {
        SendStatusMessage("错误: 机械臂未连接");
        return false;
    }

    SendStatusMessage("测试移动到准备位置...");

    errno_t ret = m_robot->MoveJByPose(
        m_readyPosition.x, m_readyPosition.y, m_readyPosition.z,
        m_readyPosition.rx, m_readyPosition.ry, m_readyPosition.rz,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("移动到准备位置失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达准备位置");
    return true;
}

bool DemoControlPresenter::TestMoveToPlace()
{
    if (!m_robotConnected) {
        SendStatusMessage("错误: 机械臂未连接");
        return false;
    }

    SendStatusMessage("测试移动到放置位置...");

    errno_t ret = m_robot->MoveJByPose(
        m_placePosition.x, m_placePosition.y, m_placePosition.z,
        m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("移动到放置位置失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达放置位置");
    return true;
}

bool DemoControlPresenter::GetCurrentPosition(double& x, double& y, double& z, double& rx, double& ry, double& rz)
{
    if (!m_robotConnected) {
        SendStatusMessage("错误: 机械臂未连接");
        return false;
    }

    errno_t ret = m_robot->GetCurrentTCPPose(x, y, z, rx, ry, rz, 0);

    if (ret != 0) {
        SendStatusMessage(QString("读取当前位置失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage(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));
    return true;
}

void DemoControlPresenter::WorkflowThreadFunc()
{
    while (!m_workflowShouldStop) {
        // 检查是否需要等待（暂停或单步模式）
        if (m_workflowPaused || m_workflowStepMode) {
            std::unique_lock<std::mutex> lock(m_stepMutex);
            m_stepCondition.wait(lock, [this]() {
                return m_workflowShouldStop || (!m_workflowPaused && !m_workflowStepMode) || m_workflowStepReady;
            });

            if (m_workflowShouldStop) break;

            // 单步模式下，执行完一步后重新等待
            if (m_workflowStepMode) {
                m_workflowStepReady = false;
            }
        }

        // 根据 m_nextStepToExecute 执行对应的步骤
        bool stepSuccess = false;
        int currentStep = m_nextStepToExecute.load();

        switch (currentStep) {
            case 0:  // 步骤1: 机械臂运行到准备位置
                stepSuccess = ExecuteMovingToReady();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "移动到准备位置失败");
                } else {
                    m_nextStepToExecute = 1;
                }
                break;

            case 1:  // 步骤2: 触发控制器检测
                stepSuccess = ExecuteTriggeringDetection();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "触发检测失败");
                } else {
                    m_nextStepToExecute = 2;
                }
                break;

            case 2:  // 步骤3: 等待检测完成
                stepSuccess = ExecuteWaitingDetectionComplete();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "等待检测完成失败");
                } else {
                    m_nextStepToExecute = 3;
                }
                break;

            case 3:  // 步骤4: 读取检测结果
                stepSuccess = ExecuteReadingResult();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "读取检测结果失败");
                } else {
                    m_nextStepToExecute = 4;
                }
                break;

            case 4:  // 步骤5: 机械臂移动到抓取位置
                stepSuccess = ExecuteMovingToGrasp();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "移动到抓取位置失败");
                } else {
                    m_nextStepToExecute = 5;
                }
                break;

            case 5:  // 步骤6: 执行抓取
                stepSuccess = ExecuteGrasping();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "执行抓取失败");
                } else {
                    m_nextStepToExecute = 6;
                }
                break;

            case 6:  // 步骤7: 机械臂移动到放置位置
                stepSuccess = ExecuteMovingToPlace();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "移动到放置位置失败");
                } else {
                    m_nextStepToExecute = 7;
                }
                break;

            case 7:  // 步骤8: 执行放置
                stepSuccess = ExecutePlacing();
                if (!stepSuccess) {
                    UpdateWorkflowStep(WorkflowStep::Error, "执行放置失败");
                } else {
                    // 完成一次循环
                    UpdateWorkflowStep(WorkflowStep::Completed, "工作流程完成");

                    // 如果是单步模式，完成后停止工作流程
                    if (m_workflowStepMode) {
                        m_nextStepToExecute = 0;  // 重置到第一步
                        m_workflowRunning = false;
                        SendStatusMessage("单步执行已完成所有步骤，下次将从第一步重新开始");
                        return;  // 退出线程
                    }

                    // 连续模式下，重置到第一步继续循环
                    m_nextStepToExecute = 0;
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                }
                break;

            default:
                // 未知步骤，重置
                m_nextStepToExecute = 0;
                break;
        }

        // 如果步骤失败，退出循环
        if (!stepSuccess) {
            break;
        }

        // 如果是单步模式，执行完一步后继续等待
        if (m_workflowStepMode && currentStep < 7) {
            continue;
        }

        // 检查是否需要停止
        if (m_workflowShouldStop) break;

        // 检查暂停
        if (m_workflowPaused) continue;
    }

    m_workflowRunning = false;
}

bool DemoControlPresenter::ExecuteMovingToReady()
{
    UpdateWorkflowStep(WorkflowStep::MovingToReady);

    errno_t ret = m_robot->MoveJByPose(
        m_readyPosition.x, m_readyPosition.y, m_readyPosition.z,
        m_readyPosition.rx, m_readyPosition.ry, m_readyPosition.rz,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("机械臂移动失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("机械臂已到达准备位置");
    return true;
}

bool DemoControlPresenter::ExecuteTriggeringDetection()
{
    UpdateWorkflowStep(WorkflowStep::TriggeringDetection);

    // 写1到地址0触发检测
    if (!WriteModbusRegister(0, 1)) {
        SendStatusMessage("触发检测失败");
        return false;
    }

    SendStatusMessage("已触发控制器检测");
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    return true;
}

bool DemoControlPresenter::ExecuteWaitingDetectionComplete()
{
    UpdateWorkflowStep(WorkflowStep::WaitingDetectionComplete);

    // 轮询地址1，等待检测完成
    const int maxRetries = 100; // 最多等待10秒
    for (int i = 0; i < maxRetries && !m_workflowShouldStop; ++i) {
        uint16_t status = 0;
        if (!ReadModbusRegister(1, status)) {
            SendStatusMessage("读取检测状态失败");
            return false;
        }

        if (status == 1) {
            SendStatusMessage("检测完成（正常）");
            return true;
        } else if (status == 2) {
            SendStatusMessage("检测完成（异常）");
            return false;
        }

        // 状态为0，继续等待
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    SendStatusMessage("等待检测完成超时");
    return false;
}

bool DemoControlPresenter::ExecuteReadingResult()
{
    UpdateWorkflowStep(WorkflowStep::ReadingResult);

    // 读取地址2-13的位姿数据（6个float = 12个寄存器）
    std::vector<uint16_t> registers;
    if (!ReadModbusRegisters(2, 12, registers)) {
        SendStatusMessage("读取检测结果失败");
        return false;
    }

    // 解析float数据
    DetectionResult result;
    result.x = IYModbusTCPClient::RegistersToFloat(registers[0], registers[1]);
    result.y = IYModbusTCPClient::RegistersToFloat(registers[2], registers[3]);
    result.z = IYModbusTCPClient::RegistersToFloat(registers[4], registers[5]);
    result.roll = IYModbusTCPClient::RegistersToFloat(registers[6], registers[7]);
    result.pitch = IYModbusTCPClient::RegistersToFloat(registers[8], registers[9]);
    result.yaw = IYModbusTCPClient::RegistersToFloat(registers[10], registers[11]);
    result.valid = true;

    {
        std::lock_guard<std::mutex> lock(m_resultMutex);
        m_lastResult = result;
    }

    emit detectionResultUpdated(result);

    SendStatusMessage(QString("检测结果: X=%1, Y=%2, Z=%3, Roll=%4, Pitch=%5, Yaw=%6")
                     .arg(result.x, 0, 'f', 2).arg(result.y, 0, 'f', 2).arg(result.z, 0, 'f', 2)
                     .arg(result.roll, 0, 'f', 2).arg(result.pitch, 0, 'f', 2).arg(result.yaw, 0, 'f', 2));

    return true;
}

bool DemoControlPresenter::ExecuteMovingToGrasp()
{
    UpdateWorkflowStep(WorkflowStep::MovingToGrasp);

    DetectionResult result;
    {
        std::lock_guard<std::mutex> lock(m_resultMutex);
        result = m_lastResult;
    }

    if (!result.valid) {
        SendStatusMessage("检测结果无效");
        return false;
    }

    // 第一步：移动到目标位置上方（安全高度）
    SendStatusMessage("移动到抓取位置上方...");
    errno_t ret = m_robot->MoveJByPose(
        result.x, result.y, result.z,
        result.roll, result.pitch, result.yaw,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("移动到抓取位置上方失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达抓取位置上方，准备下降...");

    // 第二步：下降到抓取位置（加上Z轴偏移）
    ret = m_robot->MoveJByPose(
        result.x, result.y, result.z + m_graspOffsetZ,
        result.roll, result.pitch, result.yaw,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("下降到抓取位置失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达抓取位置");
    return true;
}

bool DemoControlPresenter::ExecuteGrasping()
{
    UpdateWorkflowStep(WorkflowStep::Grasping);

    // 这里可以添加夹爪控制逻辑
    // 目前仅模拟等待
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    SendStatusMessage("抓取完成");
    return true;
}

bool DemoControlPresenter::ExecuteMovingToPlace()
{
    UpdateWorkflowStep(WorkflowStep::MovingToPlace);

    DetectionResult result;
    {
        std::lock_guard<std::mutex> lock(m_resultMutex);
        result = m_lastResult;
    }

    if (!result.valid) {
        SendStatusMessage("检测结果无效");
        return false;
    }

    // 第一步：从抓取位置抬起到安全高度
    SendStatusMessage("从抓取位置抬起...");
    errno_t ret = m_robot->MoveJByPose(
        result.x, result.y, result.z,
        result.roll, result.pitch, result.yaw,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("抬起失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已抬起到安全高度，准备移动到放置位置上方...");

    // 第二步：移动到放置位置上方（安全高度）
    ret = m_robot->MoveJByPose(
        m_placePosition.x, m_placePosition.y, m_placePosition.z - m_graspOffsetZ,
        m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("移动到放置位置上方失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达放置位置上方，准备下降...");

    // 第三步：下降到放置位置
    ret = m_robot->MoveJByPose(
        m_placePosition.x, m_placePosition.y, m_placePosition.z,
        m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
        static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
        m_toolNumber, 0
    );

    if (ret != 0) {
        SendStatusMessage(QString("下降到放置位置失败: 错误码 %1").arg(ret));
        return false;
    }

    SendStatusMessage("已到达放置位置");
    return true;
}

bool DemoControlPresenter::ExecutePlacing()
{
    UpdateWorkflowStep(WorkflowStep::Placing);

    // 这里可以添加夹爪释放逻辑
    // 目前仅模拟等待
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    SendStatusMessage("放置完成");
    return true;
}

void DemoControlPresenter::UpdateWorkflowStep(WorkflowStep step, const QString& message)
{
    {
        std::lock_guard<std::mutex> lock(m_stepMutex);
        m_currentStep = step;
    }

    QString msg = message.isEmpty() ? WorkflowStepToString(step) : message;
    emit workflowStepChanged(step, msg);
    SendStatusMessage(msg);
}

void DemoControlPresenter::SendStatusMessage(const QString& message)
{
    emit statusMessageUpdated(message);
}

bool DemoControlPresenter::ReadModbusRegister(int address, uint16_t& value)
{
    if (!m_modbusClient || !m_controllerConnected) {
        return false;
    }

    std::vector<uint16_t> values;
    auto result = m_modbusClient->ReadHoldingRegisters(address, 1, values);
    if (result != IYModbusTCPClient::SUCCESS || values.empty()) {
        return false;
    }

    value = values[0];
    return true;
}

bool DemoControlPresenter::WriteModbusRegister(int address, uint16_t value)
{
    if (!m_modbusClient || !m_controllerConnected) {
        return false;
    }

    auto result = m_modbusClient->WriteSingleRegister(address, value);
    return (result == IYModbusTCPClient::SUCCESS);
}

bool DemoControlPresenter::ReadModbusRegisters(int startAddress, int quantity, std::vector<uint16_t>& values)
{
    if (!m_modbusClient || !m_controllerConnected) {
        return false;
    }

    auto result = m_modbusClient->ReadHoldingRegisters(startAddress, quantity, values);
    return (result == IYModbusTCPClient::SUCCESS);
}