GrabBag/Tools/ControlApp/DemoControlPresenter.h

#ifndef DEMOCONTROLPRESENTER_H
#define DEMOCONTROLPRESENTER_H

#include <QObject>
#include <QString>
#include <QTimer>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <memory>
#include "IYModbusTCPClient.h"
#include "FairinoRobotWrapper.h"

/**
 * @brief 工作流程步骤枚举
 */
enum class WorkflowStep {
    Idle = 0,                   // 空闲
    MovingToReady,              // 机械臂运行到准备位置
    TriggeringDetection,        // 触发控制器检测
    WaitingDetectionComplete,   // 等待检测完成
    ReadingResult,              // 读取检测结果
    MovingToGrasp,              // 机械臂移动到抓取位置
    Grasping,                   // 执行抓取
    MovingToPlace,              // 机械臂移动到放置位置
    Placing,                    // 执行放置
    Completed,                  // 完成
    Error                       // 错误
};

/**
 * @brief 工作流程步骤转字符串
 */
inline QString WorkflowStepToString(WorkflowStep step) {
    switch (step) {
        case WorkflowStep::Idle: return "空闲";
        case WorkflowStep::MovingToReady: return "机械臂运行到准备位置";
        case WorkflowStep::TriggeringDetection: return "触发控制器检测";
        case WorkflowStep::WaitingDetectionComplete: return "等待检测完成";
        case WorkflowStep::ReadingResult: return "读取检测结果";
        case WorkflowStep::MovingToGrasp: return "机械臂移动到抓取位置";
        case WorkflowStep::Grasping: return "执行抓取";
        case WorkflowStep::MovingToPlace: return "机械臂移动到放置位置";
        case WorkflowStep::Placing: return "执行放置";
        case WorkflowStep::Completed: return "完成";
        case WorkflowStep::Error: return "错误";
        default: return "未知";
    }
}

/**
 * @brief 检测结果数据结构
 */
struct DetectionResult {
    float x = 0.0f;      // X坐标 (mm)
    float y = 0.0f;      // Y坐标 (mm)
    float z = 0.0f;      // Z坐标 (mm)
    float roll = 0.0f;   // Roll角度 (°)
    float pitch = 0.0f;  // Pitch角度 (°)
    float yaw = 0.0f;    // Yaw角度 (°)
    bool valid = false;  // 数据是否有效
};

/**
 * @brief 演示控制Presenter
 *
 * 负责协调ModbusTCP客户端和Fairino机械臂，执行完整的工作流程
 */
class DemoControlPresenter : public QObject
{
    Q_OBJECT

public:
    explicit DemoControlPresenter(QObject *parent = nullptr);
    ~DemoControlPresenter();

    // 初始化
    int Init();

    // 连接控制器（ModbusTCP客户端）
    bool ConnectToController(const QString& ip, int port = 502);

    // 断开控制器连接
    void DisconnectController();

    // 连接机械臂
    bool ConnectToRobot(const QString& ip);

    // 断开机械臂连接
    void DisconnectRobot();

    // 获取连接状态
    bool IsControllerConnected() const;
    bool IsRobotConnected() const;

    // 开始工作流程
    void StartWorkflow();

    // 停止工作流程
    void StopWorkflow();

    // 暂停工作流程
    void PauseWorkflow();

    // 恢复工作流程
    void ResumeWorkflow();

    // 单步执行
    void StepWorkflow();

    // 获取当前工作流程步骤
    WorkflowStep GetCurrentStep() const { return m_currentStep; }

    // 获取工作流程状态
    bool IsWorkflowRunning() const { return m_workflowRunning; }
    bool IsWorkflowPaused() const { return m_workflowPaused; }
    bool IsWorkflowStepMode() const { return m_workflowStepMode; }

    // 配置参数设置
    void SetReadyPosition(double x, double y, double z, double rx, double ry, double rz);
    void SetPlacePosition(double x, double y, double z, double rx, double ry, double rz);
    void SetGraspOffset(double offsetZ); // 抓取时的Z轴偏移
    void SetToolNumber(int tool); // 设置末端工具号
    void SetVelocity(double velocity); // 设置速度比例 (0-100%)
    void SetAcceleration(double acceleration); // 设置加速度比例 (0-100%)

    // 测试移动方法
    bool TestMoveToReady();
    bool TestMoveToPlace();

    // 获取当前位置
    bool GetCurrentPosition(double& x, double& y, double& z, double& rx, double& ry, double& rz);

signals:
    // 状态更新信号
    void statusMessageUpdated(const QString& message);

    // 工作流程步骤变化信号
    void workflowStepChanged(WorkflowStep step, const QString& message);

    // 控制器连接状态变化信号
    void controllerConnectionChanged(bool connected);

    // 机械臂连接状态变化信号
    void robotConnectionChanged(bool connected);

    // 检测结果更新信号
    void detectionResultUpdated(const DetectionResult& result);

private:
    // 工作流程线程函数
    void WorkflowThreadFunc();

    // 各步骤执行函数
    bool ExecuteMovingToReady();
    bool ExecuteTriggeringDetection();
    bool ExecuteWaitingDetectionComplete();
    bool ExecuteReadingResult();
    bool ExecuteMovingToGrasp();
    bool ExecuteGrasping();
    bool ExecuteMovingToPlace();
    bool ExecutePlacing();

    // 更新工作流程步骤
    void UpdateWorkflowStep(WorkflowStep step, const QString& message = "");

    // 发送状态消息
    void SendStatusMessage(const QString& message);

    // ModbusTCP读写辅助函数
    bool ReadModbusRegister(int address, uint16_t& value);
    bool WriteModbusRegister(int address, uint16_t value);
    bool ReadModbusRegisters(int startAddress, int quantity, std::vector<uint16_t>& values);

private:
    // ModbusTCP客户端
    IYModbusTCPClient* m_modbusClient = nullptr;
    std::atomic<bool> m_controllerConnected{false};

    // Fairino机械臂
    std::unique_ptr<FairinoRobotWrapper> m_robot;
    std::atomic<bool> m_robotConnected{false};

    // 工作流程控制
    std::atomic<bool> m_workflowRunning{false};
    std::atomic<bool> m_workflowShouldStop{false};
    std::atomic<bool> m_workflowPaused{false};
    std::atomic<bool> m_workflowStepMode{false};
    std::atomic<bool> m_workflowStepReady{false};
    std::thread m_workflowThread;
    WorkflowStep m_currentStep = WorkflowStep::Idle;
    std::atomic<int> m_nextStepToExecute{0};  // 记录下一步要执行的步骤（0-7对应8个步骤）
    std::mutex m_stepMutex;
    std::condition_variable m_stepCondition;

    // 检测结果
    DetectionResult m_lastResult;
    std::mutex m_resultMutex;

    // 配置参数
    struct {
        double x = 0.0, y = 0.0, z = 500.0;
        double rx = 0.0, ry = 0.0, rz = 0.0;
    } m_readyPosition;

    struct {
        double x = 500.0, y = 0.0, z = 200.0;
        double rx = 0.0, ry = 0.0, rz = 0.0;
    } m_placePosition;

    double m_graspOffsetZ = -50.0; // 抓取时Z轴偏移（向下）

    // 运动参数
    int m_toolNumber = 0; // 末端工具号
    double m_velocity = 50.0; // 速度比例 (0-100%)
    double m_acceleration = 50.0; // 加速度比例 (0-100%)
};

#endif // DEMOCONTROLPRESENTER_H