/**
 * @file test_trajectory_o6_task_cli_config.cpp
 * @brief RM75 轨迹与 LinkerHand O6 抓握任务 CLI 纯解析单元测试。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @details
 * 测试只在内存中构造 argc/argv，覆盖正常参数、必填项、O6 六关节姿态、抓握速度、
 * 抓握转矩、重复选项和帮助文本。测试不读取轨迹或安全配置，不连接 RM75/O6，
 * 也不执行任何运动。
 */
#include "rm_control/config/trajectory_o6_task_cli_config.h"

#include <array>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

namespace {

using CliConfigResult =
    rm_control::common::Result<rm_control::config::TrajectoryO6TaskCliConfig>;

CliConfigResult ParseArguments(std::vector<std::string> owned_arguments) {
    std::vector<char *> argv;
    argv.reserve(owned_arguments.size());
    for (std::string &argument : owned_arguments) {
        argv.push_back(argument.data());
    }
    return rm_control::config::ParseTrajectoryO6TaskCliArguments(
        static_cast<int>(argv.size()), argv.data());
}

std::vector<std::string> MakeValidArguments() {
    return {
        "rm_trajectory_o6_grasp",
        "trajectories/vendor/source.txt",
        "--sample-period-ms",
        "20",
        "--grip-pose",
        "1,2,3,4,5,255",
        "--grip-speed",
        "0,10,20,30,40,255",
        "--grip-torque",
        "255,200,150,100,50,0",
    };
}

bool ReplaceOptionValue(std::vector<std::string> *arguments,
                        const std::string &option_name,
                        const std::string &replacement_value) {
    if (arguments == nullptr) {
        return false;
    }
    for (std::size_t index = 0; index + 1 < arguments->size(); ++index) {
        if ((*arguments)[index] == option_name) {
            (*arguments)[index + 1] = replacement_value;
            return true;
        }
    }
    return false;
}

bool RemoveOptionAndValue(std::vector<std::string> *arguments,
                          const std::string &option_name) {
    if (arguments == nullptr) {
        return false;
    }
    for (std::size_t index = 0; index + 1 < arguments->size(); ++index) {
        if ((*arguments)[index] == option_name) {
            arguments->erase(arguments->begin() + static_cast<std::ptrdiff_t>(index),
                             arguments->begin() + static_cast<std::ptrdiff_t>(index + 2));
            return true;
        }
    }
    return false;
}

class TrajectoryO6TaskCliConfigTestSuite {
public:
    int Run() {
        testNormalArgumentsUseSafeDefaults();
        testAllOptionalArgumentsAndLeftHand();
        testMissingRequiredArgumentsAreRejected();
        testInvalidO6JointValueDimensionsAreRejected();
        testInvalidO6JointValueRangesAreRejected();
        testNonDecimalO6JointValuesAreRejected();
        testDuplicateO6JointValueOptionsAreRejected();
        testRoundTripArgumentsUseTwoDistinctProgramSlots();
        testRoundTripArgumentsRejectMissingOrUnpairedOptions();
        testDuplicateRoundTripOptionsAreRejected();
        testRoundTripProgramIdsMustDiffer();
        testInvalidSpeedIsRejected();
        testInvalidSideIsRejected();
        testNumericBoundariesAreValidated();
        testHelpDoesNotRequireTaskArguments();
        testUsageListsAllRequiredO6JointValues();
        return failures_;
    }

private:
    int failures_ = 0;

    void expect(bool condition, const std::string &message) {
        if (!condition) {
            ++failures_;
            std::cerr << "[FAIL] " << message << '\n';
        }
    }

    void testNormalArgumentsUseSafeDefaults() {
        const auto parsed = ParseArguments(MakeValidArguments());

        expect(parsed.ok, "normal task arguments should parse");
        if (!parsed.ok) {
            return;
        }
        expect(parsed.value.trajectory_path ==
                   std::filesystem::path("trajectories/vendor/source.txt"),
               "trajectory source path should be preserved");
        expect(parsed.value.sample_period_ms == 20, "sample period should be parsed in ms");
        expect(parsed.value.origin_speed_percent == 5, "origin speed should safely default to 5%");
        expect(parsed.value.plan_speed_percent == 5, "plan speed should safely default to 5%");
        expect(parsed.value.program_id == 1, "program id should default to one");
        expect(!parsed.value.return_from_trajectory_end,
               "reverse return should remain disabled by default");
        expect(parsed.value.return_program_id == 0,
               "disabled reverse return should keep its program id unset");
        expect(parsed.value.o6_side == rm_control::config::O6HandSide::Right,
               "O6 side should default to right");
        expect(static_cast<std::uint8_t>(parsed.value.o6_side) == 0x27,
               "right O6 side should map to device address 0x27");
        expect(parsed.value.grip_pose ==
                   (std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
                       1, 2, 3, 4, 5, 255}),
               "six O6 grip pose values should be parsed without reordering");
        expect(parsed.value.grip_speed ==
                   (std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
                       0, 10, 20, 30, 40, 255}),
               "six O6 grip speed values should be parsed without reordering");
        expect(parsed.value.grip_torque ==
                   (std::array<std::uint8_t, rm_control::config::kLinkerHandO6JointCount>{
                       255, 200, 150, 100, 50, 0}),
               "six O6 grip torque values should be parsed without reordering");
        expect(!parsed.value.live_requested, "task should default to DryRun authorization");
        expect(parsed.value.safety_config_path.filename() == "drag_teach_safety.json",
               "default safety config filename should be stable");
    }

    void testAllOptionalArgumentsAndLeftHand() {
        const auto parsed = ParseArguments({
            "rm_trajectory_o6_grasp",
            "--sample-period-ms",
            "1000",
            "--origin-speed-percent",
            "10",
            "--plan-speed-percent",
            "1",
            "--program-id",
            "100",
            "--return-from-trajectory-end",
            "--return-program-id",
            "1",
            "--o6-side",
            "left",
            "--grip-pose",
            "0,10,20,30,40,255",
            "--grip-speed",
            "255,40,30,20,10,0",
            "--grip-torque",
            "1,2,3,4,5,6",
            "--live",
            "--safety-config",
            "config/custom_safety.json",
            "trajectory.txt",
        });

        expect(parsed.ok, "complete optional task arguments should parse");
        if (!parsed.ok) {
            return;
        }
        expect(parsed.value.origin_speed_percent == 10, "origin speed upper boundary parsed");
        expect(parsed.value.plan_speed_percent == 1, "plan speed lower boundary parsed");
        expect(parsed.value.program_id == 100, "program id upper boundary parsed");
        expect(parsed.value.return_from_trajectory_end,
               "explicit reverse-return flag should enable round-trip execution");
        expect(parsed.value.return_program_id == 1,
               "return program id lower boundary should parse");
        expect(parsed.value.o6_side == rm_control::config::O6HandSide::Left,
               "left O6 side should parse");
        expect(static_cast<std::uint8_t>(parsed.value.o6_side) == 0x28,
               "left O6 side should map to device address 0x28");
        expect(parsed.value.live_requested, "--live should require explicit presence");
        expect(parsed.value.safety_config_path ==
                   std::filesystem::path("config/custom_safety.json"),
               "custom safety path should be preserved");
    }

    void testMissingRequiredArgumentsAreRejected() {
        expectRequiredOptionRejected("--sample-period-ms", "sample period");
        expectRequiredOptionRejected("--grip-pose", "grip pose");
        expectRequiredOptionRejected("--grip-speed", "grip speed");
        expectRequiredOptionRejected("--grip-torque", "grip torque");

        std::vector<std::string> arguments = MakeValidArguments();
        arguments.erase(arguments.begin() + 1);
        const auto missing_trajectory = ParseArguments(arguments);
        expect(!missing_trajectory.ok, "missing trajectory path should be rejected");
    }

    void expectRequiredOptionRejected(const std::string &option_name,
                                      const std::string &case_name) {
        std::vector<std::string> arguments = MakeValidArguments();
        const bool removed = RemoveOptionAndValue(&arguments, option_name);
        expect(removed, "test baseline should contain " + option_name);
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok && parsed.error_message.find(option_name) != std::string::npos,
               "missing " + case_name + " should be rejected with option context");
    }

    void testInvalidO6JointValueDimensionsAreRejected() {
        expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5", "five-value pose");
        expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,6,7", "seven-value pose");
        expectO6JointValueOptionRejected("--grip-speed", "1,2,3,4,5", "five-value speed");
        expectO6JointValueOptionRejected("--grip-speed", "1,2,3,4,5,6,7", "seven-value speed");
        expectO6JointValueOptionRejected("--grip-torque", "1,2,3,4,5", "five-value torque");
        expectO6JointValueOptionRejected(
            "--grip-torque", "1,2,3,4,5,6,7", "seven-value torque");
    }

    void testInvalidO6JointValueRangesAreRejected() {
        expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,256", "high pose");
        expectO6JointValueOptionRejected("--grip-pose", "1,2,3,4,-1,6", "negative pose");
        expectO6JointValueOptionRejected("--grip-speed", "256,2,3,4,5,6", "high speed");
        expectO6JointValueOptionRejected("--grip-speed", "1,-1,3,4,5,6", "negative speed");
        expectO6JointValueOptionRejected("--grip-torque", "1,2,256,4,5,6", "high torque");
        expectO6JointValueOptionRejected(
            "--grip-torque", "1,2,3,-1,5,6", "negative torque");
    }

    void testNonDecimalO6JointValuesAreRejected() {
        expectO6JointValueOptionRejected(
            "--grip-pose", "1,2,three,4,5,6", "non-decimal pose");
        expectO6JointValueOptionRejected(
            "--grip-speed", "1,2,3,4,5,6.0", "floating-point speed");
        expectO6JointValueOptionRejected(
            "--grip-torque", "1,2,3,4,5,0x10", "hexadecimal torque");
    }

    void expectO6JointValueOptionRejected(const std::string &option_name,
                                           const std::string &value,
                                           const std::string &case_name) {
        std::vector<std::string> arguments = MakeValidArguments();
        const bool replaced = ReplaceOptionValue(&arguments, option_name, value);
        expect(replaced, "test baseline should contain " + option_name);
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok, case_name + " should be rejected");
    }

    void testDuplicateO6JointValueOptionsAreRejected() {
        expectDuplicateO6JointValueOptionRejected("--grip-pose", "1,2,3,4,5,6");
        expectDuplicateO6JointValueOptionRejected("--grip-speed", "6,5,4,3,2,1");
        expectDuplicateO6JointValueOptionRejected("--grip-torque", "10,20,30,40,50,60");
    }

    void testRoundTripArgumentsUseTwoDistinctProgramSlots() {
        std::vector<std::string> arguments = MakeValidArguments();
        arguments.push_back("--return-from-trajectory-end");
        arguments.push_back("--return-program-id");
        arguments.push_back("2");

        const auto parsed = ParseArguments(arguments);

        expect(parsed.ok, "paired round-trip arguments with distinct program ids should parse");
        if (parsed.ok) {
            expect(parsed.value.return_from_trajectory_end,
                   "round-trip flag should be preserved in parsed config");
            expect(parsed.value.program_id == 1 && parsed.value.return_program_id == 2,
                   "forward and return program ids should be preserved without reordering");
        }
    }

    void testRoundTripArgumentsRejectMissingOrUnpairedOptions() {
        std::vector<std::string> missing_return_id = MakeValidArguments();
        missing_return_id.push_back("--return-from-trajectory-end");
        const auto missing_id = ParseArguments(missing_return_id);
        expect(!missing_id.ok &&
                   missing_id.error_message.find("--return-program-id") != std::string::npos,
               "reverse-return flag without a return program id should be rejected");

        std::vector<std::string> return_id_without_flag = MakeValidArguments();
        return_id_without_flag.push_back("--return-program-id");
        return_id_without_flag.push_back("2");
        const auto unpaired_id = ParseArguments(return_id_without_flag);
        expect(!unpaired_id.ok && unpaired_id.error_message.find(
                   "--return-from-trajectory-end") != std::string::npos,
               "return program id without the reverse-return flag should be rejected");
    }

    void testDuplicateRoundTripOptionsAreRejected() {
        std::vector<std::string> duplicate_flag = MakeValidArguments();
        duplicate_flag.push_back("--return-from-trajectory-end");
        duplicate_flag.push_back("--return-program-id");
        duplicate_flag.push_back("2");
        duplicate_flag.push_back("--return-from-trajectory-end");
        const auto duplicate_flag_result = ParseArguments(duplicate_flag);
        expect(!duplicate_flag_result.ok && duplicate_flag_result.error_message.find(
                   "duplicate option") != std::string::npos,
               "duplicate reverse-return flag should be rejected explicitly");

        std::vector<std::string> duplicate_id = MakeValidArguments();
        duplicate_id.push_back("--return-from-trajectory-end");
        duplicate_id.push_back("--return-program-id");
        duplicate_id.push_back("2");
        duplicate_id.push_back("--return-program-id");
        duplicate_id.push_back("3");
        const auto duplicate_id_result = ParseArguments(duplicate_id);
        expect(!duplicate_id_result.ok && duplicate_id_result.error_message.find(
                   "duplicate option") != std::string::npos,
               "duplicate return program id should be rejected explicitly");
    }

    void testRoundTripProgramIdsMustDiffer() {
        std::vector<std::string> arguments = MakeValidArguments();
        arguments.push_back("--return-from-trajectory-end");
        arguments.push_back("--return-program-id");
        arguments.push_back("1");

        const auto parsed = ParseArguments(arguments);

        expect(!parsed.ok && parsed.error_message.find("must differ") != std::string::npos,
               "return program id must differ from the default forward program id");
    }

    void expectDuplicateO6JointValueOptionRejected(const std::string &option_name,
                                                    const std::string &value) {
        std::vector<std::string> arguments = MakeValidArguments();
        arguments.push_back(option_name);
        arguments.push_back(value);
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok && parsed.error_message.find("duplicate option") != std::string::npos,
               "duplicate " + option_name + " should be rejected explicitly");
    }

    void testInvalidSpeedIsRejected() {
        expectIntegerOptionRejected("--origin-speed-percent", "0", "zero origin speed");
        expectIntegerOptionRejected("--origin-speed-percent", "11", "high origin speed");
        expectIntegerOptionRejected("--plan-speed-percent", "0", "zero plan speed");
        expectIntegerOptionRejected("--plan-speed-percent", "11", "high plan speed");
    }

    void expectIntegerOptionRejected(const std::string &option,
                                     const std::string &value,
                                     const std::string &case_name) {
        std::vector<std::string> arguments = MakeValidArguments();
        if (!ReplaceOptionValue(&arguments, option, value)) {
            arguments.push_back(option);
            arguments.push_back(value);
        }
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok, case_name + " should be rejected");
    }

    void testInvalidSideIsRejected() {
        std::vector<std::string> arguments = MakeValidArguments();
        arguments.push_back("--o6-side");
        arguments.push_back("center");
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok, "unknown O6 side should be rejected");
    }

    void testNumericBoundariesAreValidated() {
        expectIntegerOptionRejected("--sample-period-ms", "0", "zero sample period");
        expectIntegerOptionRejected("--sample-period-ms", "1001", "high sample period");
        expectIntegerOptionRejected("--program-id", "0", "zero program id");
        expectIntegerOptionRejected("--program-id", "101", "high program id");
        expectReturnProgramIdRejected("0", "zero return program id");
        expectReturnProgramIdRejected("101", "high return program id");
    }

    void expectReturnProgramIdRejected(const std::string &value,
                                       const std::string &case_name) {
        std::vector<std::string> arguments = MakeValidArguments();
        arguments.push_back("--return-from-trajectory-end");
        arguments.push_back("--return-program-id");
        arguments.push_back(value);
        const auto parsed = ParseArguments(arguments);
        expect(!parsed.ok && parsed.error_message.find("--return-program-id") != std::string::npos,
               case_name + " should be rejected with return option context");
    }

    void testHelpDoesNotRequireTaskArguments() {
        const auto short_help = ParseArguments({"rm_trajectory_o6_grasp", "-h"});
        expect(short_help.ok && short_help.value.help_requested,
               "short help should not require task arguments");

        const auto long_help = ParseArguments({"rm_trajectory_o6_grasp", "--help"});
        expect(long_help.ok && long_help.value.help_requested,
               "long help should not require task arguments");
    }

    void testUsageListsAllRequiredO6JointValues() {
        std::ostringstream usage;
        rm_control::config::PrintTrajectoryO6TaskUsage("rm_trajectory_o6_grasp", usage);
        const std::string text = usage.str();
        expect(text.find("--grip-pose v1,v2,v3,v4,v5,v6") != std::string::npos,
               "usage should list the required six-value grip pose");
        expect(text.find("--grip-speed v1,v2,v3,v4,v5,v6") != std::string::npos,
               "usage should list the required six-value grip speed");
        expect(text.find("--grip-torque v1,v2,v3,v4,v5,v6") != std::string::npos,
               "usage should list the required six-value grip torque");
        expect(text.find("all required; no execution defaults") != std::string::npos,
               "usage should state that O6 action values have no execution defaults");
        expect(text.find("--return-from-trajectory-end --return-program-id N") !=
                   std::string::npos,
               "usage should document paired reverse-return arguments");
        expect(text.find("two controller slots") != std::string::npos,
               "usage should explain that round-trip execution reserves two program slots");
    }
};

} // namespace

/**
 * @brief 运行 RM75 轨迹与 O6 抓握任务 CLI 单元测试。
 *
 * @reason 使用独立无外部依赖测试程序，让 CTest 能在没有 RM75、O6 或厂商 SDK 时验证解析。
 *
 * @author 唐永康
 * @date 2026-07-11
 *
 * @return 0 表示全部断言通过；1 表示至少一个断言失败。
 *
 * @calls 只调用纯命令行解析函数，不访问文件系统内容、网络或硬件。
 * @safety 本测试永远不连接设备，也不会执行真实运动。
 * @note 测试失败信息写入标准错误流，便于 CTest 输出定位。
 */
int main() {
    TrajectoryO6TaskCliConfigTestSuite suite;
    const int failures = suite.Run();
    if (failures == 0) {
        std::cout << "[PASS] rm_trajectory_o6_task_cli_config_tests\n";
        return 0;
    }
    std::cerr << failures << " test(s) failed\n";
    return 1;
}
