#include "rm_control/config/json_command.h"

#include "rm_control/config/simple_json.h"

#include <cmath>
#include <fstream>
#include <sstream>

namespace rm_control::config {
namespace {

using common::Result;

Result<void> requireObject(const JsonValue &value, const std::string &name) {
    // 命令文件顶层和 connection/safety/command 都必须是对象。
    if (!value.isObject()) {
        return Result<void>::failure(name + " must be an object");
    }
    return Result<void>::success();
}

Result<std::string> requiredString(const JsonValue &object,
                                   const std::string &field,
                                   const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr || !value->isString()) {
        return Result<std::string>::failure(context + "." + field + " must be a string");
    }
    return Result<std::string>::success(value->asString());
}

Result<double> requiredNumber(const JsonValue &object,
                              const std::string &field,
                              const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr || !value->isNumber() || !std::isfinite(value->asNumber())) {
        return Result<double>::failure(context + "." + field + " must be a finite number");
    }
    return Result<double>::success(value->asNumber());
}

Result<void> optionalString(const JsonValue &object,
                            const std::string &field,
                            std::string *target,
                            const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr) {
        return Result<void>::success();
    }
    if (!value->isString()) {
        return Result<void>::failure(context + "." + field + " must be a string");
    }
    *target = value->asString();
    return Result<void>::success();
}

Result<void> optionalInt(const JsonValue &object,
                         const std::string &field,
                         int *target,
                         const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr) {
        return Result<void>::success();
    }
    if (!value->isNumber() || !std::isfinite(value->asNumber())) {
        return Result<void>::failure(context + "." + field + " must be a finite number");
    }
    *target = static_cast<int>(value->asNumber());
    return Result<void>::success();
}

Result<void> optionalFloat(const JsonValue &object,
                           const std::string &field,
                           float *target,
                           const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr) {
        return Result<void>::success();
    }
    if (!value->isNumber() || !std::isfinite(value->asNumber())) {
        return Result<void>::failure(context + "." + field + " must be a finite number");
    }
    *target = static_cast<float>(value->asNumber());
    return Result<void>::success();
}

Result<void> optionalBool(const JsonValue &object,
                          const std::string &field,
                          bool *target,
                          const std::string &context) {
    const JsonValue *value = object.find(field);
    if (value == nullptr) {
        return Result<void>::success();
    }
    if (!value->isBool()) {
        return Result<void>::failure(context + "." + field + " must be a bool");
    }
    *target = value->asBool();
    return Result<void>::success();
}

Result<std::vector<float>> requiredNumberArray(const JsonValue &object,
                                               const std::string &field,
                                               const std::string &context) {
    // 关节角数组必须全部是有限数字，不能出现 null/string/NaN。
    const JsonValue *value = object.find(field);
    if (value == nullptr || !value->isArray()) {
        return Result<std::vector<float>>::failure(context + "." + field + " must be an array");
    }

    std::vector<float> numbers;
    for (const JsonValue &item : value->asArray()) {
        if (!item.isNumber() || !std::isfinite(item.asNumber())) {
            return Result<std::vector<float>>::failure(context + "." + field + " must contain finite numbers");
        }
        numbers.push_back(static_cast<float>(item.asNumber()));
    }
    return Result<std::vector<float>>::success(numbers);
}

Result<CommandType> parseCommandType(const std::string &type) {
    // JSON 文本中的 type 和内部枚举在这里集中映射。
    // 后续新增命令时，需要同时更新 commandTypeName 和测试。
    if (type == "get_robot_info") {
        return Result<CommandType>::success(CommandType::GetRobotInfo);
    }
    if (type == "get_current_arm_state") {
        return Result<CommandType>::success(CommandType::GetCurrentArmState);
    }
    if (type == "get_force_data") {
        return Result<CommandType>::success(CommandType::GetForceData);
    }
    if (type == "sleep_ms") {
        return Result<CommandType>::success(CommandType::SleepMs);
    }
    if (type == "movej") {
        return Result<CommandType>::success(CommandType::MoveJ);
    }
    if (type == "movej_delta") {
        return Result<CommandType>::success(CommandType::MoveJDelta);
    }
    if (type == "movej_delta_steps") {
        return Result<CommandType>::success(CommandType::MoveJDeltaSteps);
    }
    if (type == "movel") {
        return Result<CommandType>::success(CommandType::MoveL);
    }
    return Result<CommandType>::failure("unsupported command type: " + type);
}

Result<common::Pose> parsePose(const JsonValue &command, const std::string &context) {
    // pose 支持两种写法：
    // 1. 数组：[x,y,z,rx,ry,rz]
    // 2. 对象：{"x":...,"y":...}
    // 对象形式更适合人工编辑，数组形式更紧凑。
    const JsonValue *pose_value = command.find("pose");
    if (pose_value == nullptr) {
        return Result<common::Pose>::failure(context + ".pose is required");
    }

    common::Pose pose;
    if (pose_value->isArray()) {
        const auto &array = pose_value->asArray();
        if (array.size() != 6) {
            return Result<common::Pose>::failure(context + ".pose array must contain [x,y,z,rx,ry,rz]");
        }
        for (const JsonValue &item : array) {
            if (!item.isNumber() || !std::isfinite(item.asNumber())) {
                return Result<common::Pose>::failure(context + ".pose must contain finite numbers");
            }
        }
        pose.x = static_cast<float>(array[0].asNumber());
        pose.y = static_cast<float>(array[1].asNumber());
        pose.z = static_cast<float>(array[2].asNumber());
        pose.rx = static_cast<float>(array[3].asNumber());
        pose.ry = static_cast<float>(array[4].asNumber());
        pose.rz = static_cast<float>(array[5].asNumber());
        return Result<common::Pose>::success(pose);
    }

    if (!pose_value->isObject()) {
        return Result<common::Pose>::failure(context + ".pose must be an object or array");
    }

    const auto x = requiredNumber(*pose_value, "x", context + ".pose");
    const auto y = requiredNumber(*pose_value, "y", context + ".pose");
    const auto z = requiredNumber(*pose_value, "z", context + ".pose");
    const auto rx = requiredNumber(*pose_value, "rx", context + ".pose");
    const auto ry = requiredNumber(*pose_value, "ry", context + ".pose");
    const auto rz = requiredNumber(*pose_value, "rz", context + ".pose");
    if (!x.ok) {
        return Result<common::Pose>::failure(x.error_message);
    }
    if (!y.ok) {
        return Result<common::Pose>::failure(y.error_message);
    }
    if (!z.ok) {
        return Result<common::Pose>::failure(z.error_message);
    }
    if (!rx.ok) {
        return Result<common::Pose>::failure(rx.error_message);
    }
    if (!ry.ok) {
        return Result<common::Pose>::failure(ry.error_message);
    }
    if (!rz.ok) {
        return Result<common::Pose>::failure(rz.error_message);
    }

    pose.x = static_cast<float>(x.value);
    pose.y = static_cast<float>(y.value);
    pose.z = static_cast<float>(z.value);
    pose.rx = static_cast<float>(rx.value);
    pose.ry = static_cast<float>(ry.value);
    pose.rz = static_cast<float>(rz.value);
    return Result<common::Pose>::success(pose);
}

Result<void> parseConnection(const JsonValue &root, common::RobotConnectionConfig *connection) {
    // connection 可省略；省略时使用 RobotConnectionConfig 的默认 RM75 地址。
    const JsonValue *value = root.find("connection");
    if (value == nullptr) {
        return Result<void>::success();
    }
    auto object_ok = requireObject(*value, "connection");
    if (!object_ok.ok) {
        return object_ok;
    }

    auto result = optionalString(*value, "ip", &connection->ip, "connection");
    if (!result.ok) {
        return result;
    }
    result = optionalInt(*value, "port", &connection->port, "connection");
    if (!result.ok) {
        return result;
    }
    result = optionalInt(*value, "timeout_ms", &connection->timeout_ms, "connection");
    if (!result.ok) {
        return result;
    }

    if (connection->ip.empty()) {
        return Result<void>::failure("connection.ip must not be empty");
    }
    if (connection->port <= 0 || connection->port > 65535) {
        return Result<void>::failure("connection.port must be in range 1..65535");
    }
    if (connection->timeout_ms <= 0) {
        return Result<void>::failure("connection.timeout_ms must be greater than 0");
    }
    return Result<void>::success();
}

Result<void> parseSafety(const JsonValue &root, SafetyOptions *safety) {
    // safety 可省略；省略时保持 dry_run=true/allow_motion=false，默认不动真机。
    const JsonValue *value = root.find("safety");
    if (value == nullptr) {
        return Result<void>::success();
    }
    auto object_ok = requireObject(*value, "safety");
    if (!object_ok.ok) {
        return object_ok;
    }

    auto result = optionalBool(*value, "dry_run", &safety->dry_run, "safety");
    if (!result.ok) {
        return result;
    }
    result = optionalBool(*value, "allow_motion", &safety->allow_motion, "safety");
    if (!result.ok) {
        return result;
    }
    result = optionalInt(*value, "max_speed_percent", &safety->max_speed_percent, "safety");
    if (!result.ok) {
        return result;
    }
    result = optionalFloat(*value, "max_joint_delta_deg", &safety->max_joint_delta_deg, "safety");
    if (!result.ok) {
        return result;
    }
    result = optionalFloat(*value, "max_cartesian_step_m", &safety->max_cartesian_step_m, "safety");
    if (!result.ok) {
        return result;
    }
    result = optionalFloat(*value, "max_euler_delta_rad", &safety->max_euler_delta_rad, "safety");
    if (!result.ok) {
        return result;
    }

    if (safety->max_speed_percent < 1 || safety->max_speed_percent > 100) {
        return Result<void>::failure("safety.max_speed_percent must be in range 1..100");
    }
    if (safety->max_joint_delta_deg <= 0.0F) {
        return Result<void>::failure("safety.max_joint_delta_deg must be positive");
    }
    if (safety->max_cartesian_step_m <= 0.0F) {
        return Result<void>::failure("safety.max_cartesian_step_m must be positive");
    }
    if (safety->max_euler_delta_rad <= 0.0F) {
        return Result<void>::failure("safety.max_euler_delta_rad must be positive");
    }
    return Result<void>::success();
}

Result<MotionOptions> parseMotionOptions(const JsonValue &command, const std::string &context) {
    // 运动参数可以写在每条命令上，未写时使用保守默认值。
    MotionOptions motion;
    auto result = optionalInt(command, "speed_percent", &motion.speed_percent, context);
    if (!result.ok) {
        return Result<MotionOptions>::failure(result.error_message);
    }
    result = optionalInt(command, "blend_radius", &motion.blend_radius, context);
    if (!result.ok) {
        return Result<MotionOptions>::failure(result.error_message);
    }
    result = optionalInt(command, "trajectory_connect", &motion.trajectory_connect, context);
    if (!result.ok) {
        return Result<MotionOptions>::failure(result.error_message);
    }
    result = optionalBool(command, "blocking", &motion.blocking, context);
    if (!result.ok) {
        return Result<MotionOptions>::failure(result.error_message);
    }

    if (motion.speed_percent < 1 || motion.speed_percent > 100) {
        return Result<MotionOptions>::failure(context + ".speed_percent must be in range 1..100");
    }
    if (motion.blend_radius < 0) {
        return Result<MotionOptions>::failure(context + ".blend_radius must not be negative");
    }
    if (motion.trajectory_connect != 0 && motion.trajectory_connect != 1) {
        return Result<MotionOptions>::failure(context + ".trajectory_connect must be 0 or 1");
    }
    return Result<MotionOptions>::success(motion);
}

Result<ArmCommand> parseCommand(const JsonValue &value, size_t index) {
    // 单条命令解析只负责把 JSON 转成结构体，不访问硬件、不做运动安全判断。
    // 运动安全判断统一放在 JsonCommandRunner::validateMotionCommand。
    const std::string context = "commands[" + std::to_string(index) + "]";
    auto object_ok = requireObject(value, context);
    if (!object_ok.ok) {
        return Result<ArmCommand>::failure(object_ok.error_message);
    }

    auto type_text = requiredString(value, "type", context);
    if (!type_text.ok) {
        return Result<ArmCommand>::failure(type_text.error_message);
    }
    auto parsed_type = parseCommandType(type_text.value);
    if (!parsed_type.ok) {
        return Result<ArmCommand>::failure(context + ": " + parsed_type.error_message);
    }

    ArmCommand command;
    command.type = parsed_type.value;
    command.name = type_text.value;
    auto optional_name = optionalString(value, "name", &command.name, context);
    if (!optional_name.ok) {
        return Result<ArmCommand>::failure(optional_name.error_message);
    }

    auto motion = parseMotionOptions(value, context);
    if (!motion.ok) {
        return Result<ArmCommand>::failure(motion.error_message);
    }
    command.motion = motion.value;

    if (command.type == CommandType::SleepMs) {
        auto duration = requiredNumber(value, "duration_ms", context);
        if (!duration.ok) {
            return Result<ArmCommand>::failure(duration.error_message);
        }
        command.duration_ms = static_cast<int>(duration.value);
        if (command.duration_ms < 0) {
            return Result<ArmCommand>::failure(context + ".duration_ms must not be negative");
        }
    } else if (command.type == CommandType::MoveJ) {
        auto joints = requiredNumberArray(value, "joints_deg", context);
        if (!joints.ok) {
            return Result<ArmCommand>::failure(joints.error_message);
        }
        if (joints.value.empty()) {
            return Result<ArmCommand>::failure(context + ".joints_deg must not be empty");
        }
        command.joints_deg = joints.value;
    } else if (command.type == CommandType::MoveJDelta ||
               command.type == CommandType::MoveJDeltaSteps) {
        auto joints = requiredNumberArray(value, "delta_deg", context);
        if (!joints.ok) {
            return Result<ArmCommand>::failure(joints.error_message);
        }
        if (joints.value.empty()) {
            return Result<ArmCommand>::failure(context + ".delta_deg must not be empty");
        }
        command.joint_delta_deg = joints.value;
        if (command.type == CommandType::MoveJDeltaSteps) {
            auto steps_result = optionalInt(value, "steps", &command.steps, context);
            if (!steps_result.ok) {
                return Result<ArmCommand>::failure(steps_result.error_message);
            }
            steps_result = optionalInt(value, "step_sleep_ms", &command.step_sleep_ms, context);
            if (!steps_result.ok) {
                return Result<ArmCommand>::failure(steps_result.error_message);
            }
            if (command.steps <= 0) {
                return Result<ArmCommand>::failure(context + ".steps must be greater than 0");
            }
            if (command.step_sleep_ms < 0) {
                return Result<ArmCommand>::failure(context + ".step_sleep_ms must not be negative");
            }
        }
    } else if (command.type == CommandType::MoveL) {
        auto pose = parsePose(value, context);
        if (!pose.ok) {
            return Result<ArmCommand>::failure(pose.error_message);
        }
        command.pose = pose.value;
    }

    return Result<ArmCommand>::success(command);
}

} // namespace

Result<JsonCommandProgram> parseCommandProgramText(const std::string &text) {
    // 顶层解析顺序：JSON 语法 -> connection -> safety -> commands。
    // 任意一步失败都返回明确错误，不继续执行后续硬件动作。
    auto root = parseJson(text);
    if (!root.ok) {
        return Result<JsonCommandProgram>::failure(root.error_message);
    }
    if (!root.value.isObject()) {
        return Result<JsonCommandProgram>::failure("root JSON value must be an object");
    }

    JsonCommandProgram program;
    auto result = parseConnection(root.value, &program.connection);
    if (!result.ok) {
        return Result<JsonCommandProgram>::failure(result.error_message);
    }
    result = parseSafety(root.value, &program.safety);
    if (!result.ok) {
        return Result<JsonCommandProgram>::failure(result.error_message);
    }

    const JsonValue *commands = root.value.find("commands");
    if (commands == nullptr || !commands->isArray()) {
        return Result<JsonCommandProgram>::failure("commands must be an array");
    }
    if (commands->asArray().empty()) {
        return Result<JsonCommandProgram>::failure("commands must not be empty");
    }

    for (size_t i = 0; i < commands->asArray().size(); ++i) {
        auto command = parseCommand(commands->asArray()[i], i);
        if (!command.ok) {
            return Result<JsonCommandProgram>::failure(command.error_message);
        }
        program.commands.push_back(command.value);
    }

    return Result<JsonCommandProgram>::success(program);
}

Result<JsonCommandProgram> loadCommandProgramFromFile(const std::string &path) {
    // 文件读取放在 config 层，main 不关心 JSON 解析细节。
    std::ifstream file(path);
    if (!file) {
        return Result<JsonCommandProgram>::failure("failed to open command file: " + path);
    }
    std::ostringstream buffer;
    buffer << file.rdbuf();
    return parseCommandProgramText(buffer.str());
}

const char *commandTypeName(CommandType type) {
    switch (type) {
    case CommandType::GetRobotInfo:
        return "get_robot_info";
    case CommandType::GetCurrentArmState:
        return "get_current_arm_state";
    case CommandType::GetForceData:
        return "get_force_data";
    case CommandType::SleepMs:
        return "sleep_ms";
    case CommandType::MoveJ:
        return "movej";
    case CommandType::MoveJDelta:
        return "movej_delta";
    case CommandType::MoveJDeltaSteps:
        return "movej_delta_steps";
    case CommandType::MoveL:
        return "movel";
    }
    return "unknown";
}

} // namespace rm_control::config
