cmvr-es/cmvr-es/applications/include/touch_screen_app.h

#pragma once

#ifndef CMVR_ES_TOUCH_SCREEN_APP_H
#define CMVR_ES_TOUCH_SCREEN_APP_H

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

#include <Eigen/Dense>

#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
#include "controller/include/ibvs_controller.h"
#include "devices/camera/abstract_camera.h"
#include "devices/dexhand/abstract_dexhand.h"
#include "devices/robot/abstract_robot.h"
#include "perception/include/apriltag_perception.h"
#include "perception/include/tag_relative_target_3d.h"

namespace cmvr::app {

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

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

    enum class AlignMode {
        POSE_AND_POSITION = 0,  // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
        RX_RY_AND_POSITION,     // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
        POSITION_ONLY           // 保留锁定时看到的完整 tag 姿态，只按位置对齐。
    };

    struct Options {
        // 是否在触控流程开始前先回到指定初始关节位姿。
        bool move_to_init_position_before_start{false};
        // 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。
        bool move_to_init_position{false};
        // 初始关节位姿目标，在前置回位或结束后回位开启时使用。
        std::vector<device::JointPoint> init_joint_positions{};
        // 回到初始位姿时的 moveJ 主导速度，单位 rad/s。
        double init_movej_vel{1.0};
        // 回到初始位姿时的 moveJ 主导加速度，单位 rad/s^2。
        double init_movej_acc{2.0};

        // IBVS / IK 初始化参数。
        // URDF 文件路径，用于初始化 IbvsController 内部 IK 求解器。
        std::string urdf_path;
        // IK 链基座 link 名称。
        std::string base_link{"PELVIS_S"};
        // IK 链末端法兰 link 名称。
        std::string flange_link{"R_WRIST_R_S"};
        // URDF 中相机 link 名称。
        std::string camera_link;

        // 视觉感知参数。
        // AprilTag 实际边长，单位米。
        double tag_size_m{0.12};
        // 感知更新时如何使用深度图：不用 / 尽量用 / 必须用。
        perception::AprilTagPerception::DepthPolicy depth_policy{
            perception::AprilTagPerception::DepthPolicy::NONE};
        // 从像素恢复目标点时采用 tag 平面求交，还是深度图反投影。
        perception::TagRelativeTarget3D::TargetPointMethod target_point_method{
            perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE};

        // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。
        // 目标点在相机坐标系中的期望位置，单位米。
        Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.40};
        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
        double target_rx{3.14159265358979323846};
        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
        double target_ry{0.0};
        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
        double target_rz{0.0};
        // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。
        AlignMode align_mode{AlignMode::POSE_AND_POSITION};

        // IBVS 参数。
        // 视觉伺服增益 lambda。
        double ibvs_lambda{0.6};
        // DLS IK 阻尼系数 mu。
        double ibvs_mu{0.1};
        // 单关节最大速度，单位 rad/s。
        double ibvs_qdot_max{0.15};
        // 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。
        std::array<double, 6> ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
        // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`。
        std::array<double, 6> ibvs_amax6{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
        // 相机 twist 一阶低通滤波系数，范围 [0, 1]。
        double ibvs_twist_filter_alpha{0.35};
        // 是否启用关节限位回避。
        bool enable_joint_limit_avoidance{true};
        // 关节限位回避增益。
        double joint_limit_avoidance_gain{0.2};
        // 距离关节限位多近时开始回避，按关节范围比例计算。
        double joint_limit_avoidance_margin_ratio{0.15};
        // 单关节限位回避最大推回速度。
        double joint_limit_avoidance_max_push{0.25};

        // `AbstractCamera` 相机坐标系到 ViSP 相机坐标系的旋转矩阵。
        Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
        // `AbstractCamera` 相机坐标系到 URDF 相机坐标系的旋转矩阵。
        Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};

        // 关节控制链，默认右臂 7 轴。
        // 顺序必须与 IbvsController 内部 IK 链顺序一致。
        std::vector<std::string> control_joint_names{
            "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
            "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};

        // 视觉对准收敛判据 `[x, y, z, rx, ry, rz]`。
        // 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。
        std::array<double, 6> align_error_threshold6{{0.003, 0.003, 0.010,
                                                       0.08726646259971647,
                                                       0.08726646259971647,
                                                       0.08726646259971647}};
        // 连续多少帧都满足阈值，才认为对准完成。
        int align_stable_frames{5};
        // 对准阶段超时时间，单位秒。
        double align_timeout_s{10.0};
        // 为 true 时对准完成后暂停，不自动进入触控阶段。
        bool pause_after_align_reached{false};

        // 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。
        // 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。
        // 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。
        bool touch_use_speedl{true};
        // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。
        // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。
        // 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。
        Eigen::Matrix<double, 6, 1> touch_twist_base{
            (Eigen::Matrix<double, 6, 1>() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()};
        // 触控阶段 speedL 的加速度参数。
        double touch_speedl_acceleration{3.0};
        // 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。
        double touch_forward_l{0.08};
        // 触控阶段使用 moveL 时的末端速度，单位 m/s。
        double touch_movel_speed{0.25};
        // 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。
        double touch_movel_acceleration{1.2};
        // 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。
        double touch_movel_jerk{5.0};
        // 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。
        std::vector<double> touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5};
        // 前进触控阶段的最大累计位移，单位米。
        // 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。
        // 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。
        // 小于等于 0 时，表示不启用这条限制。
        double touch_speedl_forward_l{0.08};

        // 接触后停留与回退。当前按末端 Tool 坐标系解释。
        // 检测到接触后在当前位置停留的时间，单位秒。
        // 当该值小于 0 时，表示不做停留，直接把 speedL 切换为回退。
        double dwell_time_s{0.05};
        // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。
        // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。
        // 回退阶段统一使用 speedL。
        Eigen::Matrix<double, 6, 1> retract_twist_base{
            (Eigen::Matrix<double, 6, 1>() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()};
        // 回退阶段 speedL 的加速度参数。
        double retract_acceleration{3.0};
        // 回退阶段 speedL 持续时间，单位秒。
        double retract_duration_s{0.8};

        // 指尖触觉判据。
        // 使用哪根手指的触觉阵列判断是否接触。
        device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
        // 使用该手指的哪个触觉区域。
        device::AbstractDexHand::TactileRegion tactile_region{
            device::AbstractDexHand::TactileRegion::TIP};
        // 触觉压力和阈值；总和超过该值认为已经接触。
        double tactile_pressure_sum_threshold{100.0};
        // 触觉峰值阈值；为 0 时表示不使用峰值判据。
        double tactile_pressure_peak_threshold{0.0};
        // 当前帧非 0 触觉点数量阈值；只有达到该数量后，才进一步判断 sum/peak 阈值。
        int tactile_nonzero_count_threshold{1};
    };

    TouchScreenApp();
    ~TouchScreenApp() = default;

    bool init();
    bool init(const std::shared_ptr<device::AbstractRobot>& robot,
              const std::shared_ptr<device::AbstractDexHand>& dexhand,
              const std::shared_ptr<device::AbstractCamera>& camera);
    bool init(const std::shared_ptr<device::AbstractRobot>& robot,
              const std::shared_ptr<device::AbstractDexHand>& dexhand,
              const std::shared_ptr<device::AbstractCamera>& camera,
              const cmvr::config::TouchScreenAppConfig& config);

    bool setOptionsFromConfig();
    bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config);
    bool setTouchSpeedlForwardL(double forward_l);

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

    Phase phase() const { return phase_; }
    Status lastStatus() const { return last_status_; }
    static const char* phaseToString(Phase phase);
    static const char* statusToString(Status status);

    bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
                                 phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
                                 phase_ == Phase::RETRACTING; }
    bool isFinished() const { return phase_ == Phase::DONE; }
    bool isFailed() const { return phase_ == Phase::FAILED; }

    int targetU() const { return target_u_; }
    int targetV() const { return target_v_; }
    double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
    double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
    int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; }
    int lastActiveTagId() const { return last_active_tag_id_; }
    const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }

    const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
    const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
    const IbvsController& ibvs() const { return ibvs_; }

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

    static bool optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config,
                                  Options& options_out);
    void setOptions(const Options& options);
    bool applyOptions();
    bool validateControlJointNames() const;
    bool stepAligning(double dt);
    bool stepTouching();
    bool stepDwelling();
    bool stepRetracting();

    bool readControlledJointPositions(std::vector<double>& q_out) const;
    bool sendJointVelocity(const std::vector<double>& qdot) const;
    bool sendZeroJointVelocity() const;
    void hardStopIbvsMotion();
    bool holdCurrentControlledPosition() const;
    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();

private:
    std::shared_ptr<device::AbstractRobot> robot_{nullptr};
    std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
    std::shared_ptr<device::AbstractCamera> camera_{nullptr};

    std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
    perception::TagRelativeTarget3D tracker_;
    IbvsController ibvs_;
    Options options_{};

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

    bool initialized_{false};
    bool target_locked_{false};
    bool ibvs_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 last_active_tag_id_{-1};

    double last_touch_pressure_sum_{0.0};
    double last_touch_pressure_peak_{0.0};
    int last_touch_nonzero_count_{0};
    Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
    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()};

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

using touch_screen_app = TouchScreenApp;

} // namespace cmvr::app

#endif // CMVR_ES_TOUCH_SCREEN_APP_H