/**
 * @file trajectory_o6_task_cli_config.cpp
 * @brief RM75 轨迹与 LinkerHand O6 抓握任务命令行参数解析实现。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @details
 * 本文件只解析和校验命令行文本，不检查文件是否存在，不连接 RM75 或 O6，也不创建
 * 输出文件。所有数值使用闭区间校验；机械臂百分比速度默认 5%，O6 姿态、速度和转矩
 * 均不提供可执行默认值，必须由命令行显式给出。
 *
 * @dependencies
 * - rm_control/config/trajectory_o6_task_cli_config.h
 */
#include "rm_control/config/trajectory_o6_task_cli_config.h"

#include <charconv>
#include <ostream>
#include <string>
#include <system_error>

#ifndef RM_PROJECT_ROOT
#define RM_PROJECT_ROOT "."
#endif

namespace rm_control::config {
namespace {

inline constexpr int kMinimumSamplePeriodMilliseconds = 1;
inline constexpr int kMaximumSamplePeriodMilliseconds = 1000;
inline constexpr int kMinimumSpeedPercent = 1;
inline constexpr int kMaximumSpeedPercent = 10;
inline constexpr int kMinimumProgramId = 1;
inline constexpr int kMaximumProgramId = 100;
inline constexpr int kMinimumO6JointValue = 0;
inline constexpr int kMaximumO6JointValue = 255;

using O6JointValues = std::array<std::uint8_t, kLinkerHandO6JointCount>;

struct ParseState {
    TrajectoryO6TaskCliConfig config;
    bool trajectory_path_seen = false;
    bool sample_period_seen = false;
    bool origin_speed_seen = false;
    bool plan_speed_seen = false;
    bool program_id_seen = false;
    bool return_from_trajectory_end_seen = false;
    bool return_program_id_seen = false;
    bool o6_side_seen = false;
    bool grip_pose_seen = false;
    bool grip_speed_seen = false;
    bool grip_torque_seen = false;
    bool live_seen = false;
    bool safety_config_seen = false;
};

struct IntegerOptionTarget {
    int *destination = nullptr;
    bool *seen = nullptr;
    int minimum_value = 0;
    int maximum_value = 0;
};

struct O6JointValuesOptionTarget {
    O6JointValues *destination = nullptr;
    bool *seen = nullptr;
};

TrajectoryO6TaskCliConfig BuildDefaultConfig() {
    TrajectoryO6TaskCliConfig config;
    config.safety_config_path =
        std::filesystem::path(RM_PROJECT_ROOT) / "config" / "drag_teach_safety.json";
    return config;
}

std::string SafeProgramName(const char *program_name) {
    if (program_name == nullptr || program_name[0] == '\0') {
        return "rm_trajectory_o6_grasp";
    }
    return program_name;
}

common::Result<int> ParseBoundedDecimalInteger(const std::string &text,
                                               const std::string &option_name,
                                               int minimum_value,
                                               int maximum_value) {
    int parsed_value = 0;
    const char *begin = text.data();
    const char *end = begin + text.size();
    const auto parse_result = std::from_chars(begin, end, parsed_value, 10);
    if (text.empty() || parse_result.ec != std::errc{} || parse_result.ptr != end) {
        return common::Result<int>::failure(
            option_name + " requires a decimal integer: " + text);
    }
    if (parsed_value < minimum_value || parsed_value > maximum_value) {
        return common::Result<int>::failure(
            option_name + " must be in range " + std::to_string(minimum_value) + ".." +
            std::to_string(maximum_value) + ": " + text);
    }
    return common::Result<int>::success(parsed_value);
}

common::Result<void> MarkOptionSeen(bool *seen, const std::string &option_name) {
    if (seen == nullptr) {
        return common::Result<void>::failure("internal parser state is invalid for " + option_name);
    }
    if (*seen) {
        return common::Result<void>::failure("duplicate option: " + option_name);
    }
    *seen = true;
    return common::Result<void>::success();
}

common::Result<std::string> TakeOptionValue(int argc,
                                            char **argv,
                                            int *argument_index,
                                            const std::string &option_name) {
    if (argument_index == nullptr || *argument_index + 1 >= argc) {
        return common::Result<std::string>::failure(option_name + " requires a value");
    }
    ++(*argument_index);
    const char *raw_value = argv[*argument_index];
    if (raw_value == nullptr || raw_value[0] == '\0') {
        return common::Result<std::string>::failure(option_name + " requires a non-empty value");
    }
    return common::Result<std::string>::success(raw_value);
}

common::Result<O6JointValues> ParseO6JointValues(const std::string &text,
                                                const std::string &option_name) {
    O6JointValues joint_values{};
    std::size_t token_begin = 0;
    for (std::size_t joint_index = 0; joint_index < joint_values.size(); ++joint_index) {
        const std::size_t comma_position = text.find(',', token_begin);
        const bool parsing_last_joint = joint_index + 1 == joint_values.size();
        if ((!parsing_last_joint && comma_position == std::string::npos) ||
            (parsing_last_joint && comma_position != std::string::npos)) {
            return common::Result<O6JointValues>::failure(
                option_name + " requires exactly six comma-separated decimal values");
        }

        const std::size_t token_end =
            comma_position == std::string::npos ? text.size() : comma_position;
        const std::string token = text.substr(token_begin, token_end - token_begin);
        const auto parsed_value = ParseBoundedDecimalInteger(
            token, option_name, kMinimumO6JointValue, kMaximumO6JointValue);
        if (!parsed_value.ok) {
            return common::Result<O6JointValues>::failure(parsed_value.error_message);
        }
        joint_values[joint_index] = static_cast<std::uint8_t>(parsed_value.value);
        token_begin = token_end + 1;
    }
    return common::Result<O6JointValues>::success(joint_values);
}

bool IsIntegerOption(const std::string &argument) {
    return argument == "--sample-period-ms" || argument == "--origin-speed-percent" ||
        argument == "--plan-speed-percent" || argument == "--program-id" ||
        argument == "--return-program-id";
}

bool IsO6JointValuesOption(const std::string &argument) {
    return argument == "--grip-pose" || argument == "--grip-speed" ||
        argument == "--grip-torque";
}

IntegerOptionTarget SelectIntegerOptionTarget(const std::string &option_name,
                                              ParseState *state) {
    if (option_name == "--sample-period-ms") {
        return {&state->config.sample_period_ms,
                &state->sample_period_seen,
                kMinimumSamplePeriodMilliseconds,
                kMaximumSamplePeriodMilliseconds};
    }
    if (option_name == "--origin-speed-percent") {
        return {&state->config.origin_speed_percent,
                &state->origin_speed_seen,
                kMinimumSpeedPercent,
                kMaximumSpeedPercent};
    }
    if (option_name == "--plan-speed-percent") {
        return {&state->config.plan_speed_percent,
                &state->plan_speed_seen,
                kMinimumSpeedPercent,
                kMaximumSpeedPercent};
    }
    if (option_name == "--program-id") {
        return {&state->config.program_id,
                &state->program_id_seen,
                kMinimumProgramId,
                kMaximumProgramId};
    }
    return {&state->config.return_program_id,
            &state->return_program_id_seen,
            kMinimumProgramId,
            kMaximumProgramId};
}

common::Result<void> ParseIntegerOption(const std::string &option_name,
                                        int argc,
                                        char **argv,
                                        int *argument_index,
                                        ParseState *state) {
    const IntegerOptionTarget target = SelectIntegerOptionTarget(option_name, state);
    const auto unique = MarkOptionSeen(target.seen, option_name);
    if (!unique.ok) {
        return unique;
    }
    const auto value = TakeOptionValue(argc, argv, argument_index, option_name);
    if (!value.ok) {
        return common::Result<void>::failure(value.error_message);
    }
    const auto parsed = ParseBoundedDecimalInteger(
        value.value, option_name, target.minimum_value, target.maximum_value);
    if (!parsed.ok) {
        return common::Result<void>::failure(parsed.error_message);
    }
    *target.destination = parsed.value;
    return common::Result<void>::success();
}

O6JointValuesOptionTarget SelectO6JointValuesOptionTarget(
    const std::string &option_name, ParseState *state) {
    if (option_name == "--grip-pose") {
        return {&state->config.grip_pose, &state->grip_pose_seen};
    }
    if (option_name == "--grip-speed") {
        return {&state->config.grip_speed, &state->grip_speed_seen};
    }
    return {&state->config.grip_torque, &state->grip_torque_seen};
}

common::Result<void> ParseO6JointValuesOption(const std::string &option_name,
                                               int argc,
                                               char **argv,
                                               int *argument_index,
                                               ParseState *state) {
    const O6JointValuesOptionTarget target =
        SelectO6JointValuesOptionTarget(option_name, state);
    const auto unique = MarkOptionSeen(target.seen, option_name);
    if (!unique.ok) {
        return unique;
    }
    const auto value = TakeOptionValue(argc, argv, argument_index, option_name);
    if (!value.ok) {
        return common::Result<void>::failure(value.error_message);
    }
    const auto joint_values = ParseO6JointValues(value.value, option_name);
    if (!joint_values.ok) {
        return common::Result<void>::failure(joint_values.error_message);
    }
    *target.destination = joint_values.value;
    return common::Result<void>::success();
}

common::Result<void> ParseO6SideOption(int argc,
                                      char **argv,
                                      int *argument_index,
                                      ParseState *state) {
    const auto unique = MarkOptionSeen(&state->o6_side_seen, "--o6-side");
    if (!unique.ok) {
        return unique;
    }
    const auto value = TakeOptionValue(argc, argv, argument_index, "--o6-side");
    if (!value.ok) {
        return common::Result<void>::failure(value.error_message);
    }
    if (value.value == "right") {
        state->config.o6_side = O6HandSide::Right;
    } else if (value.value == "left") {
        state->config.o6_side = O6HandSide::Left;
    } else {
        return common::Result<void>::failure("--o6-side must be right or left: " + value.value);
    }
    return common::Result<void>::success();
}

common::Result<void> ParseSafetyConfigOption(int argc,
                                             char **argv,
                                             int *argument_index,
                                             ParseState *state) {
    const auto unique = MarkOptionSeen(&state->safety_config_seen, "--safety-config");
    if (!unique.ok) {
        return unique;
    }
    const auto value = TakeOptionValue(argc, argv, argument_index, "--safety-config");
    if (!value.ok) {
        return common::Result<void>::failure(value.error_message);
    }
    state->config.safety_config_path = value.value;
    return common::Result<void>::success();
}

common::Result<void> ParseLiveOption(ParseState *state) {
    const auto unique = MarkOptionSeen(&state->live_seen, "--live");
    if (!unique.ok) {
        return unique;
    }
    state->config.live_requested = true;
    return common::Result<void>::success();
}

common::Result<void> ParseReturnFromTrajectoryEndOption(ParseState *state) {
    const auto unique = MarkOptionSeen(
        &state->return_from_trajectory_end_seen, "--return-from-trajectory-end");
    if (!unique.ok) {
        return unique;
    }
    state->config.return_from_trajectory_end = true;
    return common::Result<void>::success();
}

common::Result<void> ParseNamedOption(const std::string &argument,
                                      int argc,
                                      char **argv,
                                      int *argument_index,
                                      ParseState *state) {
    if (IsIntegerOption(argument)) {
        return ParseIntegerOption(argument, argc, argv, argument_index, state);
    }
    if (IsO6JointValuesOption(argument)) {
        return ParseO6JointValuesOption(argument, argc, argv, argument_index, state);
    }
    if (argument == "--o6-side") {
        return ParseO6SideOption(argc, argv, argument_index, state);
    }
    if (argument == "--safety-config") {
        return ParseSafetyConfigOption(argc, argv, argument_index, state);
    }
    if (argument == "--live") {
        return ParseLiveOption(state);
    }
    if (argument == "--return-from-trajectory-end") {
        return ParseReturnFromTrajectoryEndOption(state);
    }
    return common::Result<void>::failure("unknown option: " + argument);
}

common::Result<void> ParseArgument(const std::string &argument,
                                   int argc,
                                   char **argv,
                                   int *argument_index,
                                   ParseState *state) {
    if (argument.empty()) {
        return common::Result<void>::failure("command-line argument must not be empty");
    }
    if (argument[0] == '-') {
        return ParseNamedOption(argument, argc, argv, argument_index, state);
    }
    if (state->trajectory_path_seen) {
        return common::Result<void>::failure(
            "expected exactly one trajectory_path; unexpected argument: " + argument);
    }
    state->config.trajectory_path = std::filesystem::path(argument);
    state->trajectory_path_seen = true;
    return common::Result<void>::success();
}

common::Result<void> ValidateRequiredArguments(const ParseState &state) {
    if (!state.trajectory_path_seen || state.config.trajectory_path.empty()) {
        return common::Result<void>::failure("missing required trajectory_path");
    }
    if (!state.sample_period_seen) {
        return common::Result<void>::failure("missing required option: --sample-period-ms");
    }
    if (!state.grip_pose_seen) {
        return common::Result<void>::failure("missing required option: --grip-pose");
    }
    if (!state.grip_speed_seen) {
        return common::Result<void>::failure("missing required option: --grip-speed");
    }
    if (!state.grip_torque_seen) {
        return common::Result<void>::failure("missing required option: --grip-torque");
    }
    if (state.return_from_trajectory_end_seen && !state.return_program_id_seen) {
        return common::Result<void>::failure(
            "--return-from-trajectory-end requires --return-program-id");
    }
    if (!state.return_from_trajectory_end_seen && state.return_program_id_seen) {
        return common::Result<void>::failure(
            "--return-program-id requires --return-from-trajectory-end");
    }
    if (state.return_from_trajectory_end_seen &&
        state.config.return_program_id == state.config.program_id) {
        return common::Result<void>::failure(
            "--return-program-id must differ from --program-id");
    }
    return common::Result<void>::success();
}

} // namespace

void PrintTrajectoryO6TaskUsage(const char *program_name, std::ostream &out) {
    out << "Usage: " << SafeProgramName(program_name)
        << " <trajectory_path> --sample-period-ms N"
        << " [--origin-speed-percent N] [--plan-speed-percent N] [--program-id N]"
        << " [--return-from-trajectory-end --return-program-id N]"
        << " [--o6-side right|left]"
        << " --grip-pose v1,v2,v3,v4,v5,v6"
        << " --grip-speed v1,v2,v3,v4,v5,v6"
        << " --grip-torque v1,v2,v3,v4,v5,v6"
        << " [--live] [--safety-config path]\n"
        << "\n"
        << "Required ranges: sample period 1..1000 ms; each O6 pose, speed, and torque value"
        << " 0..255.\n"
        << "O6 pose, speed, and torque are all required; no execution defaults are used.\n"
        << "Reverse return: --return-from-trajectory-end requires a distinct"
        << " --return-program-id so reverse and forward trajectories use two controller slots.\n"
        << "Safe defaults: DryRun, reverse return disabled, origin/plan speed 5%,"
        << " program 1, right O6 (0x27).\n"
        << "Safety: --live is only one prerequisite; the complete hardware gate must still pass.\n";
}

common::Result<TrajectoryO6TaskCliConfig>
ParseTrajectoryO6TaskCliArguments(int argc, char **argv) {
    ParseState state;
    state.config = BuildDefaultConfig();
    if (argc < 0 || (argc > 0 && argv == nullptr)) {
        return common::Result<TrajectoryO6TaskCliConfig>::failure("invalid argc/argv input");
    }
    if (argc == 2 && argv[1] != nullptr) {
        const std::string first_argument = argv[1];
        if (first_argument == "-h" || first_argument == "--help") {
            state.config.help_requested = true;
            return common::Result<TrajectoryO6TaskCliConfig>::success(state.config);
        }
    }

    for (int argument_index = 1; argument_index < argc; ++argument_index) {
        const std::string argument = argv[argument_index] == nullptr ? "" : argv[argument_index];
        const auto parsed = ParseArgument(argument, argc, argv, &argument_index, &state);
        if (!parsed.ok) {
            return common::Result<TrajectoryO6TaskCliConfig>::failure(parsed.error_message);
        }
    }
    const auto required = ValidateRequiredArguments(state);
    if (!required.ok) {
        return common::Result<TrajectoryO6TaskCliConfig>::failure(required.error_message);
    }
    return common::Result<TrajectoryO6TaskCliConfig>::success(state.config);
}

} // namespace rm_control::config
