/**
 * @file json_command_runner.cpp
 * @brief RM JSON 指令执行器实现文件。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 本文件实现 JSON 指令执行流程。执行器只依赖 `IRobotArm` 抽象接口，不包含 RealMan
 * SDK 头文件。真实运动命令会先读取当前机械臂状态，再执行速度、关节增量、笛卡尔位移
 * 和欧拉角变化限幅；`dry_run=true` 时只校验不发送运动。
 *
 * 设计原则：
 * - 顶层 `run()` 只负责连接和命令循环；
 * - 只读命令、运动命令和各运动类型拆成独立函数；
 * - 所有运动命令必须经过同一个安全入口；
 * - 错误通过 `Result` 返回，不抛异常给入口文件。
 *
 * @dependencies
 * - rm_control/core/json_command_runner.h
 */
#include "rm_control/core/json_command_runner.h"

#include <chrono>
#include <cmath>
#include <iomanip>
#include <ostream>
#include <thread>
#include <vector>

namespace rm_control::core {
namespace {

/**
 * @brief 计算两个浮点数的绝对差值。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 安全校验需要比较目标值和当前值的差值。该函数封装 `std::fabs`，
 * 让关节和笛卡尔校验代码表达更清晰。
 *
 * @param[in] left 左侧数值。
 * @param[in] right 右侧数值。
 *
 * @return 两个数值的绝对差。
 *
 * @throws 本函数不主动抛出异常。
 *
 * @note
 * 调用方负责保证输入为有限值；JSON 解析层和驱动读取层已做基础约束。
 *
 * @warning
 * 如果输入 NaN，返回值也会是 NaN，比较逻辑不会触发大于阈值分支。
 *
 * @thread_safety
 * 函数无全局状态，线程安全。
 *
 * @par 边界条件
 * 正负零差值返回 0。
 *
 * @par 后续维护
 * 若引入统一数学工具模块，可迁移该函数。
 */
float absDiff(float left, float right) {
    return std::fabs(left - right);
}

/**
 * @brief 将 TCP 位姿写入日志流。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 多个命令分支都需要输出 TCP 位姿。该函数集中格式化位置和欧拉角，
 * 避免输出格式在不同分支中漂移。
 *
 * @param[out] out 日志输出流。
 * @param[in] pose TCP 位姿，位置单位为 m，欧拉角单位为 rad。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 输出精度固定为 4 位小数，适合人工查看。
 *
 * @warning
 * 修改输出格式可能影响依赖日志文本的外部脚本。
 *
 * @thread_safety
 * 函数自身无状态；并发写同一输出流需要调用方保护。
 *
 * @par 边界条件
 * 零位姿按固定格式输出。
 *
 * @par 后续维护
 * 如果需要机器可读日志，应新增结构化日志接口，不要复用此人工格式。
 */
void printPose(std::ostream &out, const common::Pose &pose) {
    out << std::fixed << std::setprecision(4)
        << "pos_m=(" << pose.x << "," << pose.y << "," << pose.z << ")"
        << ", euler_rad=(" << pose.rx << "," << pose.ry << "," << pose.rz << ")";
}

/**
 * @brief 将关节角列表写入日志流。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 关节状态和关节运动目标都需要统一输出。该函数按 `J1=...` 的形式输出，
 * 便于人工对照 RM75 的 7 轴状态。
 *
 * @param[out] out 日志输出流。
 * @param[in] joints 关节角列表，单位为度。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 函数不强制关节数量，实际数量由调用方输入决定。
 *
 * @warning
 * 修改输出格式可能影响外部日志解析脚本。
 *
 * @thread_safety
 * 函数自身无状态；并发写同一输出流需要调用方保护。
 *
 * @par 边界条件
 * 空关节列表输出 `[]`。
 *
 * @par 后续维护
 * 如果后续支持非度单位，应在调用前转换，而不是让本函数判断单位。
 */
void printJoints(std::ostream &out, const std::vector<float> &joints) {
    out << std::fixed << std::setprecision(3) << "[";
    for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) {
        if (joint_index > 0) {
            out << ", ";
        }
        out << "J" << (joint_index + 1) << "=" << joints[joint_index];
    }
    out << "]";
}

/**
 * @brief 打印 JSON 程序启动摘要。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 执行前统一输出连接目标和安全配置，便于用户确认当前 JSON 是否仍处于 dry-run
 * 或是否允许真实运动。该函数只写日志，不连接硬件。
 *
 * @param[in] program JSON 指令程序。
 * @param[out] out 日志输出流。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 输出内容是人工可读诊断信息，不作为稳定机器协议。
 *
 * @warning
 * 不要在这里触发任何硬件状态读取，保持启动日志纯输出。
 *
 * @thread_safety
 * 函数无全局状态；并发写同一输出流需要调用方保护。
 *
 * @par 边界条件
 * 空命令列表也会输出连接和 safety 摘要。
 *
 * @par 后续维护
 * 新增 safety 字段时应同步输出，避免运行日志遗漏关键安全开关。
 */
void printProgramSummary(const config::JsonCommandProgram &program, std::ostream &out) {
    out << "Connecting to RealMan arm at " << program.connection.ip << ':'
        << program.connection.port << " timeout=" << program.connection.timeout_ms << " ms\n";
    out << "Safety: dry_run=" << (program.safety.dry_run ? "true" : "false")
        << ", allow_motion=" << (program.safety.allow_motion ? "true" : "false")
        << ", max_speed_percent=" << program.safety.max_speed_percent
        << ", max_joint_delta_deg=" << program.safety.max_joint_delta_deg
        << ", max_cartesian_step_m=" << program.safety.max_cartesian_step_m << '\n';
}

/**
 * @brief 基于当前关节角和相对增量生成绝对关节目标。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * RM_API2 的 `rm_movej` 接收绝对关节角。JSON 为人工调试提供相对增量命令，
 * 因此执行前需要把当前关节值和增量相加，生成实际发送给驱动的目标。
 *
 * @param[in] current_joints_deg 当前关节角，单位为度。
 * @param[in] delta_deg 相对关节增量，单位为度，长度应与当前关节角一致。
 *
 * @return 绝对关节目标列表。
 *
 * @throws 本函数不主动抛出异常；向量分配失败由标准库报告。
 *
 * @note
 * 长度一致性由安全校验函数提前保证。
 *
 * @warning
 * 如果调用方跳过长度校验，较短 delta 会造成越界；因此本函数只在校验后使用。
 *
 * @thread_safety
 * 函数只使用局部变量，线程安全。
 *
 * @par 边界条件
 * 空输入返回空输出。
 *
 * @par 后续维护
 * 如果引入关节限位，应在安全校验层处理，不应让本函数承担策略判断。
 */
std::vector<float> buildAbsoluteJointTarget(const std::vector<float> &current_joints_deg,
                                            const std::vector<float> &delta_deg) {
    std::vector<float> target = current_joints_deg;
    for (size_t joint_index = 0; joint_index < target.size(); ++joint_index) {
        target[joint_index] += delta_deg[joint_index];
    }
    return target;
}

} // namespace

JsonCommandRunner::JsonCommandRunner(interfaces::IRobotArm &robot)
    : robot_(robot) {}

common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &program,
                                            std::ostream &out) {
    printProgramSummary(program, out);

    const auto connect_result = robot_.connect(program.connection);
    if (!connect_result.ok) {
        return connect_result;
    }

    for (size_t command_index = 0; command_index < program.commands.size(); ++command_index) {
        const auto result = runSingleCommand(program,
                                             program.commands[command_index],
                                             command_index,
                                             program.commands.size(),
                                             out);
        if (!result.ok) {
            return result;
        }
    }

    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::runSingleCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    size_t command_index,
    size_t command_count,
    std::ostream &out) {
    out << "\n[" << (command_index + 1) << "/" << command_count << "] "
        << command.name << " (" << config::commandTypeName(command.type) << ")\n";

    switch (command.type) {
    case config::CommandType::GetRobotInfo:
    case config::CommandType::GetCurrentArmState:
    case config::CommandType::GetForceData:
        return runReadOnlyCommand(command, out);
    case config::CommandType::SleepMs:
        out << "sleep " << command.duration_ms << " ms\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(command.duration_ms));
        return common::Result<void>::success();
    case config::CommandType::MoveJ:
    case config::CommandType::MoveJDelta:
    case config::CommandType::MoveJDeltaSteps:
    case config::CommandType::MoveL:
        return runMotionCommand(program, command, out);
    }

    return common::Result<void>::failure("unsupported command enum value");
}

common::Result<void> JsonCommandRunner::runReadOnlyCommand(const config::ArmCommand &command,
                                                           std::ostream &out) {
    if (command.type == config::CommandType::GetRobotInfo) {
        const auto info = robot_.getRobotInfo();
        if (!info.ok) {
            return common::Result<void>::failure(info.error_message);
        }
        out << "arm_dof=" << info.value.arm_dof
            << ", arm_model=" << info.value.arm_model
            << ", force_type=" << info.value.force_type
            << ", controller_version=" << info.value.controller_version << '\n';
        return common::Result<void>::success();
    }

    if (command.type == config::CommandType::GetCurrentArmState) {
        const auto state = robot_.getCurrentArmState();
        if (!state.ok) {
            return common::Result<void>::failure(state.error_message);
        }
        out << "joints_deg=";
        printJoints(out, state.value.joints_deg);
        out << ", tcp_pose=";
        printPose(out, state.value.tcp_pose);
        out << ", error_count=" << state.value.error_codes.size() << '\n';
        return common::Result<void>::success();
    }

    if (command.type == config::CommandType::GetForceData) {
        const auto force = robot_.getForceData();
        if (!force.ok) {
            return common::Result<void>::failure(force.error_message);
        }
        out << std::fixed << std::setprecision(4)
            << "work_zero_force=[Fx=" << force.value.work_zero[0]
            << ", Fy=" << force.value.work_zero[1]
            << ", Fz=" << force.value.work_zero[2]
            << ", Mx=" << force.value.work_zero[3]
            << ", My=" << force.value.work_zero[4]
            << ", Mz=" << force.value.work_zero[5] << "]\n";
        return common::Result<void>::success();
    }

    return common::Result<void>::failure("command is not a read-only command");
}

common::Result<void> JsonCommandRunner::runMotionCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    std::ostream &out) {
    const auto current_state = robot_.getCurrentArmState();
    if (!current_state.ok) {
        return common::Result<void>::failure(current_state.error_message);
    }

    const auto validation = validateMotionCommand(program, command, current_state.value);
    if (!validation.ok) {
        return validation;
    }

    if (program.safety.dry_run) {
        out << "dry_run=true, motion command validated but not sent.\n";
        return common::Result<void>::success();
    }
    if (!program.safety.allow_motion) {
        return common::Result<void>::failure(
            "motion command refused because safety.allow_motion is false");
    }

    switch (command.type) {
    case config::CommandType::MoveJ:
        return executeMoveJCommand(command, out);
    case config::CommandType::MoveJDelta:
        return executeMoveJDeltaCommand(command, current_state.value, out);
    case config::CommandType::MoveJDeltaSteps:
        return executeMoveJDeltaStepsCommand(command, out);
    case config::CommandType::MoveL:
        return executeMoveLCommand(command, out);
    default:
        return common::Result<void>::failure("command is not a motion command");
    }
}

common::Result<void> JsonCommandRunner::validateMotionCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    const common::ArmState &current_state) const {
    if (command.motion.speed_percent > program.safety.max_speed_percent) {
        return common::Result<void>::failure("motion speed exceeds safety.max_speed_percent");
    }

    switch (command.type) {
    case config::CommandType::MoveJ:
        return validateMoveJCommand(program, command, current_state);
    case config::CommandType::MoveJDelta:
        return validateMoveJDeltaCommand(program, command, current_state);
    case config::CommandType::MoveJDeltaSteps:
        return validateMoveJDeltaStepsCommand(program, command, current_state);
    case config::CommandType::MoveL:
        return validateMoveLCommand(program, command, current_state);
    default:
        return common::Result<void>::failure("command is not a motion command");
    }
}

common::Result<void> JsonCommandRunner::validateMoveJCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    const common::ArmState &current_state) const {
    if (command.joints_deg.size() != current_state.joints_deg.size()) {
        return common::Result<void>::failure("movej joints_deg size must match current robot DOF");
    }

    for (size_t joint_index = 0; joint_index < command.joints_deg.size(); ++joint_index) {
        const float delta = absDiff(command.joints_deg[joint_index],
                                    current_state.joints_deg[joint_index]);
        if (delta > program.safety.max_joint_delta_deg) {
            return common::Result<void>::failure(
                "movej J" + std::to_string(joint_index + 1) + " delta " +
                std::to_string(delta) + " deg exceeds safety.max_joint_delta_deg");
        }
    }
    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::validateMoveJDeltaCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    const common::ArmState &current_state) const {
    if (command.joint_delta_deg.size() != current_state.joints_deg.size()) {
        return common::Result<void>::failure("movej_delta delta_deg size must match current robot DOF");
    }

    for (size_t joint_index = 0; joint_index < command.joint_delta_deg.size(); ++joint_index) {
        const float delta = std::fabs(command.joint_delta_deg[joint_index]);
        if (delta > program.safety.max_joint_delta_deg) {
            return common::Result<void>::failure(
                "movej_delta J" + std::to_string(joint_index + 1) + " delta " +
                std::to_string(delta) + " deg exceeds safety.max_joint_delta_deg");
        }
    }
    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::validateMoveJDeltaStepsCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    const common::ArmState &current_state) const {
    if (command.joint_delta_deg.size() != current_state.joints_deg.size()) {
        return common::Result<void>::failure(
            "movej_delta_steps delta_deg size must match current robot DOF");
    }
    if (command.steps <= 0) {
        return common::Result<void>::failure("movej_delta_steps steps must be greater than 0");
    }

    for (size_t joint_index = 0; joint_index < command.joint_delta_deg.size(); ++joint_index) {
        const float per_step_delta =
            std::fabs(command.joint_delta_deg[joint_index]) / static_cast<float>(command.steps);
        if (per_step_delta > program.safety.max_joint_delta_deg) {
            return common::Result<void>::failure(
                "movej_delta_steps J" + std::to_string(joint_index + 1) +
                " per-step delta " + std::to_string(per_step_delta) +
                " deg exceeds safety.max_joint_delta_deg");
        }
    }
    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::validateMoveLCommand(
    const config::JsonCommandProgram &program,
    const config::ArmCommand &command,
    const common::ArmState &current_state) const {
    const float dx = absDiff(command.pose.x, current_state.tcp_pose.x);
    const float dy = absDiff(command.pose.y, current_state.tcp_pose.y);
    const float dz = absDiff(command.pose.z, current_state.tcp_pose.z);
    if (dx > program.safety.max_cartesian_step_m ||
        dy > program.safety.max_cartesian_step_m ||
        dz > program.safety.max_cartesian_step_m) {
        return common::Result<void>::failure("movel Cartesian delta exceeds safety.max_cartesian_step_m");
    }

    const float drx = absDiff(command.pose.rx, current_state.tcp_pose.rx);
    const float dry = absDiff(command.pose.ry, current_state.tcp_pose.ry);
    const float drz = absDiff(command.pose.rz, current_state.tcp_pose.rz);
    if (drx > program.safety.max_euler_delta_rad ||
        dry > program.safety.max_euler_delta_rad ||
        drz > program.safety.max_euler_delta_rad) {
        return common::Result<void>::failure("movel Euler delta exceeds safety.max_euler_delta_rad");
    }

    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::executeMoveJCommand(const config::ArmCommand &command,
                                                            std::ostream &out) {
    out << "sending rm_movej target=";
    printJoints(out, command.joints_deg);
    out << ", speed=" << command.motion.speed_percent << "%\n";
    return robot_.moveJ(command.joints_deg,
                        command.motion.speed_percent,
                        command.motion.blend_radius,
                        command.motion.trajectory_connect,
                        command.motion.blocking);
}

common::Result<void> JsonCommandRunner::executeMoveJDeltaCommand(
    const config::ArmCommand &command,
    const common::ArmState &current_state,
    std::ostream &out) {
    const std::vector<float> target =
        buildAbsoluteJointTarget(current_state.joints_deg, command.joint_delta_deg);
    out << "sending rm_movej relative delta=";
    printJoints(out, command.joint_delta_deg);
    out << ", target=";
    printJoints(out, target);
    out << ", speed=" << command.motion.speed_percent << "%\n";
    return robot_.moveJ(target,
                        command.motion.speed_percent,
                        command.motion.blend_radius,
                        command.motion.trajectory_connect,
                        command.motion.blocking);
}

common::Result<void> JsonCommandRunner::executeMoveJDeltaStepsCommand(
    const config::ArmCommand &command,
    std::ostream &out) {
    out << "sending stepped rm_movej total_delta=";
    printJoints(out, command.joint_delta_deg);
    out << ", steps=" << command.steps
        << ", speed=" << command.motion.speed_percent << "%\n";

    for (int step_index = 0; step_index < command.steps; ++step_index) {
        const auto step_state = robot_.getCurrentArmState();
        if (!step_state.ok) {
            return common::Result<void>::failure(step_state.error_message);
        }

        std::vector<float> per_step_delta = command.joint_delta_deg;
        for (float &joint_delta : per_step_delta) {
            joint_delta /= static_cast<float>(command.steps);
        }
        const std::vector<float> target =
            buildAbsoluteJointTarget(step_state.value.joints_deg, per_step_delta);

        out << "  step " << (step_index + 1) << "/" << command.steps << " target=";
        printJoints(out, target);
        out << '\n';

        const auto move_result = robot_.moveJ(target,
                                             command.motion.speed_percent,
                                             command.motion.blend_radius,
                                             command.motion.trajectory_connect,
                                             command.motion.blocking);
        if (!move_result.ok) {
            return move_result;
        }
        if (command.step_sleep_ms > 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(command.step_sleep_ms));
        }
    }

    return common::Result<void>::success();
}

common::Result<void> JsonCommandRunner::executeMoveLCommand(const config::ArmCommand &command,
                                                            std::ostream &out) {
    out << "sending rm_movel target=";
    printPose(out, command.pose);
    out << ", speed=" << command.motion.speed_percent << "%\n";
    return robot_.moveL(command.pose,
                        command.motion.speed_percent,
                        command.motion.blend_radius,
                        command.motion.trajectory_connect,
                        command.motion.blocking);
}

} // namespace rm_control::core
