#pragma once

#include "rm_control/common/result.h"
#include "rm_control/core/hardware_execution_gate.h"
#include "rm_control/interfaces/dexterous_hand.h"
#include "rm_control/interfaces/robot_arm.h"
#include "rm_control/modules/trajectory/drag_trajectory_validator.h"
#include "rm_control/modules/trajectory/vendor_drag_trajectory_parser.h"

#include <chrono>
#include <cstddef>
#include <cstdint>
#include <functional>

namespace rm_control::core {

/**
 * @brief RM75 厂商轨迹执行后驱动 O6 握夹的安全参数。
 *
 * @reason 将运动速度、位置/力限制、O6 速度/转矩上限、停止回调和轮询超时作为显式输入，
 *         状态机无需读取配置文件，并可在 Mock 测试中复现每一项拒绝条件。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @safety 所有运动速度仍受 HardwareExecutionGateInput 中的配置上限约束；默认值不授权 Live。
 * @note 机械臂角度单位为 degree，力为 newton，力矩为 newton_meter，时间为 millisecond。
 */
struct ArmO6TrajectoryTaskConfig {
    modules::trajectory::DragTrajectoryValidationLimits validation_limits;
    int origin_speed_percent = 5;
    int plan_speed_percent = 5;
    int controller_program_id = 1;
    int return_controller_program_id = 0;
    interfaces::O6JointPose grip_pose{};
    interfaces::O6JointPose grip_speed_commands{};
    interfaces::O6JointPose grip_torque_commands{};
    std::uint16_t maximum_o6_speed_command = 0U;
    std::uint16_t maximum_o6_torque_command = 0U;
    double maximum_origin_delta_deg = 20.0;
    double origin_position_tolerance_deg = 1.0;
    double maximum_absolute_force_newton = 100.0;
    double maximum_absolute_torque_newton_meter = 20.0;
    std::uint16_t grip_position_tolerance = 5U;
    std::chrono::milliseconds origin_status_poll_period{100};
    std::chrono::milliseconds origin_motion_timeout{300000};
    std::chrono::milliseconds program_status_poll_period{100};
    std::chrono::milliseconds program_execution_timeout{300000};
    std::chrono::milliseconds hand_status_poll_period{100};
    std::chrono::milliseconds hand_motion_timeout{20000};
    std::function<bool()> stop_requested;
};

/**
 * @brief 纯校验 RM75 轨迹与 O6 抓握任务配置。
 *
 * @reason DryRun 和 Live 必须复用同一组位置、速度、转矩、配置上限及超时规则，避免
 *         DryRun 在返回前漏报只有真实状态机才会发现的危险参数。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @param config 待校验的任务配置；函数只读且不保留引用。
 * @return 全部字段符合 RM75/O6 范围和配置上限时成功，否则返回首个上下文错误。
 *
 * @calls 仅调用标准库数值和定长数组校验函数。
 * @safety 纯校验，不读取文件、不连接设备，也不触发机械臂或 O6 动作。
 * @note O6 位置、速度和转矩每项均限制为 0..255，速度和转矩还不得超过安全配置上限。
 */
common::Result<void> ValidateArmO6TrajectoryTaskConfig(
    const ArmO6TrajectoryTaskConfig &config);

/**
 * @brief 在基础任务参数之外校验反向与正向两个控制器程序编号。
 * @reason DryRun 必须在连接硬件前拒绝缺失、越界或相同的双程序槽配置。
 * @author 唐永康
 * @date 2026-07-11
 * @param config 包含正向和反向程序编号的任务配置。
 * @return 基础参数和两个互异的 1..100 程序编号均有效时成功。
 * @calls 仅调用纯参数校验，不访问文件或硬件。
 * @safety 纯校验，不触发运动。
 */
common::Result<void> ValidateArmO6RoundTripTaskConfig(
    const ArmO6TrajectoryTaskConfig &config);

/**
 * @brief 控制器轨迹状态轮询的单调时钟统计。
 *
 * @reason 将实际轮询周期、抖动和观察时长写入成功报告，便于离线判断主机监视是否按
 *         配置节拍工作；这些统计不冒充控制器内部真实轨迹下发周期。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @safety 仅包含 `std::chrono::steady_clock` 的观测结果，不触发任何硬件动作。
 * @note 除 poll_count 外的字段单位均为 millisecond；少于两次轮询时周期统计为零。
 */
struct ControllerProgramTimingReport {
    std::size_t poll_count = 0U;
    double minimum_poll_period_ms = 0.0;
    double maximum_poll_period_ms = 0.0;
    double average_poll_period_ms = 0.0;
    double maximum_absolute_poll_jitter_ms = 0.0;
    double observed_execution_duration_ms = 0.0;
};

/**
 * @brief 一次 RM75 轨迹与 O6 握夹任务的可审计结果。
 *
 * @reason 只在完整状态机成功后返回轨迹统计、控制器终态和 O6 读回，避免调用方把
 *         “上传成功”误记为“机械臂和灵巧手任务完成”。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @safety 报告不触发动作；只有 `Run` 成功时其中所有字段才有效。
 * @note `observed_running_state` 必须为 true，控制器终态必须为 raw_state=0。
 */
struct ArmO6TrajectoryTaskReport {
    modules::trajectory::DragTrajectoryValidationReport trajectory_validation;
    common::ControllerProgramRunState return_final_program_state;
    common::ControllerProgramRunState final_program_state;
    interfaces::DexterousHandReadiness final_hand_readiness;
    ControllerProgramTimingReport return_controller_timing;
    ControllerProgramTimingReport controller_timing;
    std::size_t trajectory_point_count = 0U;
    bool returned_from_trajectory_end = false;
    bool return_observed_running_state = false;
    bool observed_running_state = false;
};

/**
 * @brief 只读复核已解析厂商轨迹源文件的类型、权限、大小和 SHA-256。
 *
 * @reason 解析完成到真实上传之间存在文件被替换或篡改的窗口；将完整性检查作为无硬件
 *         纯函数，状态机可在首点运动前和上传紧前重复执行同一 fail-closed 规则。
 *
 * @author 唐永康
 * @date 2026-07-10
 *
 * @param trajectory 解析阶段记录了源路径、字节数和 SHA-256 的只读轨迹元数据。
 * @return 文件为非符号链接普通文件、没有任何写权限且大小与摘要均一致时成功，否则失败。
 *
 * @calls std::filesystem::symlink_status、file_size 和 CalculateFileSha256。
 * @safety 仅离线读取文件，不修改权限、内容或时间戳，也不连接或驱动任何硬件。
 * @note 该检查不能授权真实动作，仍须与完整 HardwareExecutionGate 和设备复检共同使用。
 */
common::Result<void> VerifySourceFileIntegrity(
    const modules::trajectory::VendorDragTrajectoryParseResult &trajectory);

/**
 * @brief 以 fail-closed 状态机编排 RM75 本地轨迹执行和 O6 握夹。
 *
 * @reason 真实机械臂运动与灵巧手命令必须经过同一业务状态机，禁止应用层绕过安全检查
 *         直接组合厂商接口。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @safety 本类会触发真实机械臂和 O6 动作；只能在完整 Live 门控、人工确认和现场安全条件下调用。
 * @note 本类不拥有设备对象，也不负责建立或释放连接。
 */
class ArmO6TrajectoryTask final {
public:
    /**
     * @brief 绑定已连接的机械臂和 O6 抽象接口。
     *
     * @reason 通过接口注入保持状态机与厂商 SDK 隔离，并允许单元测试完全使用 Mock。
     *
     * @author 唐永康
     * @date 2026-07-11
     *
     * @param robot RM75 接口引用，必须比本对象存活更久。
     * @param hand O6 接口引用，必须比本对象存活更久。
     * @return 构造函数无返回值。
     *
     * @calls 不调用设备接口。
     * @safety 仅保存引用，不连接设备，也不触发动作。
     * @note 调用方负责保证两个接口对应同一套现场硬件。
     */
    ArmO6TrajectoryTask(interfaces::IRobotArm &robot,
                        interfaces::IDexterousHand &hand) noexcept;

    /**
     * @brief 校验轨迹与设备后，移动到首点、执行厂商轨迹并命令 O6 握夹。
     *
     * @reason 用单一入口固定“全量校验→首点→轨迹运行态确认→结束→O6 再确认→握夹读回”顺序，
     *         任一未知状态均停止并返回上下文错误。
     *
     * @author 唐永康
     * @date 2026-07-11
     *
     * @param trajectory 已严格解析且保留只读源路径、七轴样本和推导时间戳的厂商轨迹。
     * @param config 速度、轨迹限制、O6 位置/速度/转矩、安全上限、停止回调和轮询超时。
     * @param gate_input 已由应用层收集的 Live、安全配置、连接、急停和人工确认事实。
     * @return 全流程完成且 O6 位置读回在容差内时返回报告；否则返回失败并在运动开始后尝试慢停止。
     *
     * @calls ValidateDragTrajectory、RequireHardwareExecutionPermission、IRobotArm 和 IDexterousHand。
     * @safety 会触发真实运动；本函数不得用于 DryRun，也不替代现场急停和人工监护。
     * @note 原始轨迹只读使用，不修改、不覆盖也不删除源文件。
     */
    common::Result<ArmO6TrajectoryTaskReport> Run(
        const modules::trajectory::VendorDragTrajectoryParseResult &trajectory,
        const ArmO6TrajectoryTaskConfig &config,
        const HardwareExecutionGateInput &gate_input);

    /**
     * @brief 从原轨迹末点出发，先执行只读反向产物回首点，再执行原轨迹并抓握。
     *
     * @reason 当前机械臂位于原轨迹末点时，禁止通过放大首点距离阈值做一次大跨度
     *         `moveJ`；两个控制器托管程序可以沿同一离散路径安全返回并正向复现。
     *
     * @author 唐永康
     * @date 2026-07-11
     *
     * @param return_trajectory 已重新解析、校验且逐行逆序生成的只读厂商轨迹。
     * @param forward_trajectory 操作者确认的原始只读厂商轨迹。
     * @param config 两个互异控制器程序编号、统一低速和 O6 抓握参数。
     * @param gate_input 已通过人工确认的完整硬件执行事实。
     * @return 两段程序均观察到运行后正常结束且 O6 抓握读回成功时返回报告。
     *
     * @calls IRobotArm::saveVendorDragTrajectoryProgram、runControllerProgram 和 IDexterousHand。
     * @safety 只允许当前位置严格贴近原轨迹末点；任一上传、状态、端点或停止检查失败时
     *         禁止后续阶段，并在运动已开始后请求 RM75 慢停。
     * @note 两个控制器程序槽均只保存不覆盖，函数不会删除任何已有或新建程序。
     */
    common::Result<ArmO6TrajectoryTaskReport> RunFromTrajectoryEnd(
        const modules::trajectory::VendorDragTrajectoryParseResult &return_trajectory,
        const modules::trajectory::VendorDragTrajectoryParseResult &forward_trajectory,
        const ArmO6TrajectoryTaskConfig &config,
        const HardwareExecutionGateInput &gate_input);

private:
    interfaces::IRobotArm &robot_;
    interfaces::IDexterousHand &hand_;
};

} // namespace rm_control::core
