cmvr-es/cmvr-es/task/touch_screen_task/include/touch_screen_task.h

#pragma once

#ifndef CMVR_ES_TOUCH_SCREEN_TASK_H
#define CMVR_ES_TOUCH_SCREEN_TASK_H

#include <array>
#include <chrono>
#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include <Eigen/Dense>

#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
#include "algorithms/perception/apriltag/include/apriltag_perception.h"
#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
#include "devices/camera/abstract_camera.h"
#include "devices/dexhand/abstract_dexhand.h"
#include "devices/arm/robot_arm.h"
#include "task/task.h"

namespace cmvr::task {

class TouchScreenTask : public Task {
public:
    enum class Phase {
        IDLE = 0,       // 空闲，尚未开始任务。
        ALIGNING,       // 视觉对准阶段：持续 PBVS 对齐目标点。
        ALIGN_REACHED,  // 视觉对准已达到阈值，等待进入下一阶段。
        TOUCHING,       // 前进触控阶段：沿设定方向向屏幕推进。
        DWELLING,       // 已检测到接触，保持当前位置短暂停留。
        RETRACTING,     // 回退阶段：沿设定回退方向离开屏幕。
        DONE,           // 整个流程成功完成。
        FAILED          // 流程失败并已停止。
    };

    enum class Status {
        IDLE = 0,                 // 空闲状态。
        NOT_INITIALIZED,          // 尚未调用 init() 完成初始化。
        INVALID_CONFIG,           // 配置非法，无法启动或应用参数。
        ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
        ALIGN_WAITING_TRACK,      // 对准阶段等待目标点跟踪恢复成功。
        ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。
        ALIGN_COMPUTE_FAILED,     // 对准阶段 PBVS 计算失败。
        ALIGN_TIMEOUT,            // 对准阶段超时仍未收敛。
        ALIGNING,                 // 正在执行视觉对准。
        ALIGN_REACHED,            // 视觉对准完成。
        TOUCHING,                 // 正在向前触控。
        TACTILE_UNAVAILABLE,      // 触觉数据不可用。
        TOUCH_TRIGGERED,          // 已检测到接触触发。
        TOUCH_FORWARD_TIMEOUT,    // 前进触控时间到，但未触发接触。
        RETRACTING,               // 正在回退离开屏幕。
        DONE,                     // 流程成功完成。
        STOPPED,                  // 被外部 stop() 主动停止。
        ROBOT_STATE_FAILED,       // 读取机器人状态失败。
        ROBOT_COMMAND_FAILED,     // 向机器人下发控制命令失败。
        TASK_BUSY                 // 已有触屏流程正在运行，新的 touch 请求被拒绝。
    };

    explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg);
    ~TouchScreenTask() = default;

    bool init() override;
    bool init(const std::shared_ptr<device::RobotArm>& arm,
              const std::shared_ptr<device::AbstractDexHand>& dexhand,
              const std::shared_ptr<device::AbstractCamera>& camera);
    bool init(const std::shared_ptr<device::RobotArm>& arm,
              const std::shared_ptr<device::AbstractDexHand>& dexhand,
              const std::shared_ptr<device::AbstractCamera>& camera,
              const std::shared_ptr<device::AbstractCamera>& external_camera);

    const std::string& id() const override { return id_; }

    bool touch(int u, int v);
    bool startFromPixel(int u, int v);
    bool step(double dt) override;
    void stop() override;

    Phase phase() const;
    Status lastStatus() const;
    static const char* phaseToString(Phase phase);
    static const char* statusToString(Status status);

    TaskState state() const override;
    bool isBusy() const override;
    bool isFinished() const override;
    bool isFailed() const override;
    std::string stateString() const override;
    std::string detailStatusString() const override;

    int targetU() const;
    int targetV() const;
    double lastTouchPressureSum() const;
    int lastTouchNonzeroCount() const;
    int lastActiveTagId() const;
    Eigen::Vector3d lastAlignErrorScreenTag() const;

    const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
    const std::shared_ptr<perception::AprilTagPerception>& externalPerception() const {
        return external_perception_;
    }
    const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
    const perception::TagRelativeTcpPose& tcpPoseTracker() const { return tcp_pose_tracker_; }
    const PbvsController& pbvs() const { return pbvs_; }

private:
    using Clock = std::chrono::steady_clock;

    static bool validateConfig(const cmvr::config::TouchScreenTaskConfig& config);
    bool isBusyUnlocked() const;
    bool startFromPixelUnlocked(int u, int v);
    void stopUnlocked();
    bool applyConfig();
    bool stepAligning(double dt);
    bool stepTouching();
    bool stepDwelling();
    bool stepRetracting();

    void stopPbvsMotion();
    bool buildInitJointPositions(std::vector<double>& positions_out) const;
    bool isAtInitPosition(const std::vector<double>& positions) const;
    bool moveToInitPositionBeforeStartIfEnabled();
    bool moveToInitPositionIfEnabled() const;
    bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
    void logTouchingSpeedLState() const;

    bool startTouchPhase();
    bool handleTouchTriggered(bool stop_forward_motion);
    bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
    void enterFailed(Status status);

    bool updateTouchPressure();
    void publishCoordinateOverlay();
    void refreshCoordinateOverlay();

private:
    mutable std::mutex mutex_;
    std::string id_;
    std::shared_ptr<device::RobotArm> arm_{nullptr};
    std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
    std::shared_ptr<device::AbstractCamera> camera_{nullptr};
    std::shared_ptr<device::AbstractCamera> external_camera_{nullptr};

    std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
    std::shared_ptr<perception::AprilTagPerception> external_perception_{nullptr};
    perception::TagRelativeTarget3D tracker_;
    perception::TagRelativeTcpPose tcp_pose_tracker_;
    PbvsController pbvs_;
    cmvr::config::TouchScreenTaskConfig config_{};
    bool config_valid_{false};

    Phase phase_{Phase::IDLE};
    Phase phase_after_retract_{Phase::DONE};
    Status last_status_{Status::NOT_INITIALIZED};

    bool initialized_{false};
    bool target_locked_{false};
    bool pbvs_target_initialized_{false};
    bool touch_command_started_{false};
    bool retract_command_started_{false};

    int target_u_{-1};
    int target_v_{-1};
    int align_stable_count_{0};
    int align_debug_count_{0};
    int pbvs_debug_count_{0};
    int last_active_tag_id_{-1};

    double last_touch_pressure_sum_{0.0};
    int last_touch_nonzero_count_{0};
    Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
    Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
    double pbvs_command_acceleration_{0.25};
    // Initialization MoveJ is skipped only when both position and velocity
    // are within these configured limits.
    double init_skip_position_tolerance_rad_{1e-3};
    double init_skip_velocity_tolerance_rad_s_{1e-2};
    bool locked_target_rotation_valid_{false};
    Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
    bool touch_start_position_valid_{false};
    Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
    bool retract_start_position_valid_{false};
    Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
    bool have_last_T_B_G_{false};
    Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
    double max_T_B_G_translation_delta_m_{0.0};
    double max_T_B_G_rotation_delta_rad_{0.0};

    Clock::time_point phase_start_time_{};
    Clock::time_point last_coordinate_overlay_update_time_{};
    Clock::time_point last_retract_log_time_{};
    Status final_status_after_retract_{Status::DONE};
};

using touch_screen_task = TouchScreenTask;

} // namespace cmvr::task

#endif // CMVR_ES_TOUCH_SCREEN_TASK_H