#include "rm_control/core/json_command_runner.h"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <thread>

namespace rm_control::core {
namespace {

float absDiff(float left, float right) {
    return std::fabs(left - right);
}

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 << ")";
}

void printJoints(std::ostream &out, const std::vector<float> &joints) {
    out << std::fixed << std::setprecision(3) << "[";
    for (size_t i = 0; i < joints.size(); ++i) {
        if (i > 0) {
            out << ", ";
        }
        out << "J" << (i + 1) << "=" << joints[i];
    }
    out << "]";
}

} // namespace

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

common::Result<void> JsonCommandRunner::run(const config::JsonCommandProgram &program, std::ostream &out) {
    // 执行顺序固定：先连接，再按 commands 顺序逐条执行。
    // 这里不做文件解析，保证 runner 可以直接用结构体做单元测试。
    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';

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

    for (size_t i = 0; i < program.commands.size(); ++i) {
        const config::ArmCommand &command = program.commands[i];
        out << "\n[" << (i + 1) << "/" << program.commands.size() << "] "
            << command.name << " (" << config::commandTypeName(command.type) << ")\n";

        switch (command.type) {
        case config::CommandType::GetRobotInfo: {
            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';
            break;
        }
        case config::CommandType::GetCurrentArmState: {
            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';
            break;
        }
        case config::CommandType::GetForceData: {
            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";
            break;
        }
        case config::CommandType::SleepMs:
            out << "sleep " << command.duration_ms << " ms\n";
            std::this_thread::sleep_for(std::chrono::milliseconds(command.duration_ms));
            break;
        case config::CommandType::MoveJ:
        case config::CommandType::MoveJDelta:
        case config::CommandType::MoveJDeltaSteps:
        case config::CommandType::MoveL: {
            auto current_state = robot_.getCurrentArmState();
            if (!current_state.ok) {
                return common::Result<void>::failure(current_state.error_message);
            }

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

            if (program.safety.dry_run) {
                // dry_run 仍然会连接并读取当前状态，以便验证安全边界；
                // 但不会发送 rm_movej/rm_movel。
                out << "dry_run=true, motion command validated but not sent.\n";
                break;
            }
            if (!program.safety.allow_motion) {
                return common::Result<void>::failure(
                    "motion command refused because safety.allow_motion is false");
            }

            common::Result<void> move_result;
            if (command.type == config::CommandType::MoveJ) {
                out << "sending rm_movej target=";
                printJoints(out, command.joints_deg);
                out << ", speed=" << command.motion.speed_percent << "%\n";
                move_result = robot_.moveJ(command.joints_deg,
                                           command.motion.speed_percent,
                                           command.motion.blend_radius,
                                           command.motion.trajectory_connect,
                                           command.motion.blocking);
            } else if (command.type == config::CommandType::MoveJDelta) {
                // 相对关节运动：每次发送前基于当前读数生成绝对目标。
                // RM_API2 的 rm_movej 需要绝对关节角，不接受相对增量。
                std::vector<float> target = current_state.value.joints_deg;
                for (size_t joint = 0; joint < target.size(); ++joint) {
                    target[joint] += command.joint_delta_deg[joint];
                }
                out << "sending rm_movej relative delta=";
                printJoints(out, command.joint_delta_deg);
                out << ", target=";
                printJoints(out, target);
                out << ", speed=" << command.motion.speed_percent << "%\n";
                move_result = robot_.moveJ(target,
                                           command.motion.speed_percent,
                                           command.motion.blend_radius,
                                           command.motion.trajectory_connect,
                                           command.motion.blocking);
            } else if (command.type == config::CommandType::MoveJDeltaSteps) {
                // 阶梯相对运动：适合把 20 度这类较大总幅度拆成多次小步，
                // 既便于观察，也避免单条命令产生大幅跳动。
                out << "sending stepped rm_movej total_delta=";
                printJoints(out, command.joint_delta_deg);
                out << ", steps=" << command.steps
                    << ", speed=" << command.motion.speed_percent << "%\n";

                move_result = common::Result<void>::success();
                for (int step = 0; step < command.steps; ++step) {
                    // 每一步都重新读取当前状态，而不是用上一条目标值递推。
                    // 这样可以吸收控制器实际到位误差，避免累计误差越来越大。
                    auto step_state = robot_.getCurrentArmState();
                    if (!step_state.ok) {
                        move_result = common::Result<void>::failure(step_state.error_message);
                        break;
                    }

                    std::vector<float> target = step_state.value.joints_deg;
                    for (size_t joint = 0; joint < target.size(); ++joint) {
                        target[joint] += command.joint_delta_deg[joint] / static_cast<float>(command.steps);
                    }

                    out << "  step " << (step + 1) << "/" << command.steps << " target=";
                    printJoints(out, target);
                    out << '\n';
                    move_result = robot_.moveJ(target,
                                               command.motion.speed_percent,
                                               command.motion.blend_radius,
                                               command.motion.trajectory_connect,
                                               command.motion.blocking);
                    if (!move_result.ok) {
                        break;
                    }
                    if (command.step_sleep_ms > 0) {
                        std::this_thread::sleep_for(std::chrono::milliseconds(command.step_sleep_ms));
                    }
                }
            } else {
                out << "sending rm_movel target=";
                printPose(out, command.pose);
                out << ", speed=" << command.motion.speed_percent << "%\n";
                move_result = robot_.moveL(command.pose,
                                           command.motion.speed_percent,
                                           command.motion.blend_radius,
                                           command.motion.trajectory_connect,
                                           command.motion.blocking);
            }
            if (!move_result.ok) {
                return move_result;
            }
            break;
        }
        }
    }

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

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");
    }

    if (command.type == config::CommandType::MoveJ) {
        // 绝对关节目标最容易写错，所以用当前关节值计算差值做限幅。
        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 i = 0; i < command.joints_deg.size(); ++i) {
            const float delta = absDiff(command.joints_deg[i], current_state.joints_deg[i]);
            if (delta > program.safety.max_joint_delta_deg) {
                return common::Result<void>::failure(
                    "movej J" + std::to_string(i + 1) + " delta " + std::to_string(delta) +
                    " deg exceeds safety.max_joint_delta_deg");
            }
        }
        return common::Result<void>::success();
    }

    if (command.type == config::CommandType::MoveJDelta) {
        // 相对关节目标直接检查每个关节的增量绝对值。
        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 i = 0; i < command.joint_delta_deg.size(); ++i) {
            const float delta = std::fabs(command.joint_delta_deg[i]);
            if (delta > program.safety.max_joint_delta_deg) {
                return common::Result<void>::failure(
                    "movej_delta J" + std::to_string(i + 1) + " delta " + std::to_string(delta) +
                    " deg exceeds safety.max_joint_delta_deg");
            }
        }
        return common::Result<void>::success();
    }

    if (command.type == config::CommandType::MoveJDeltaSteps) {
        // 阶梯运动允许总幅度较大，但每一步仍必须满足安全步进限制。
        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 i = 0; i < command.joint_delta_deg.size(); ++i) {
            const float per_step_delta = std::fabs(command.joint_delta_deg[i]) /
                                         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(i + 1) + " per-step delta " +
                    std::to_string(per_step_delta) +
                    " deg exceeds safety.max_joint_delta_deg");
            }
        }
        return common::Result<void>::success();
    }

    const float dx = absDiff(command.pose.x, current_state.tcp_pose.x);
    // 笛卡尔运动按 xyz 和欧拉角分别限幅，避免误把 m/rad 写成 mm/deg。
    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();
}

} // namespace rm_control::core
