#include "rm_control/config/json_command.h"
#include "rm_control/core/json_command_runner.h"
#include "rm_control/drivers/realman/realman_robot_arm.h"

#include <cstdlib>
#include <iostream>
#include <string>

namespace {

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name << " <commands.json> [--validate-only]\n"
              << "\n"
              << "Examples:\n"
              << "  " << program_name << " ../config/sample_commands.json --validate-only\n"
              << "  " << program_name << " ../config/sample_commands.json\n"
              << "\n"
              << "Safety:\n"
              << "  JSON defaults should keep dry_run=true and allow_motion=false first.\n"
              << "  Set safety.dry_run=false and safety.allow_motion=true only after checking the target.\n";
}

} // namespace

int main(int argc, char **argv) {
    // main 只负责参数解析、加载 JSON、创建具体驱动并启动 runner。
    // 具体安全校验和硬件调用都放在 core/drivers 中，避免入口文件膨胀。
    if (argc < 2 || argc > 3) {
        printUsage(argv[0]);
        return 64;
    }

    const std::string first_arg = argv[1] == nullptr ? "" : argv[1];
    if (first_arg == "-h" || first_arg == "--help") {
        printUsage(argv[0]);
        return 0;
    }

    bool validate_only = false;
    if (argc == 3) {
        const std::string option = argv[2] == nullptr ? "" : argv[2];
        if (option != "--validate-only") {
            std::cerr << "Unknown option: " << option << '\n';
            printUsage(argv[0]);
            return 64;
        }
        validate_only = true;
    }

    auto program = rm_control::config::loadCommandProgramFromFile(first_arg);
    if (!program.ok) {
        std::cerr << "Failed to load JSON command file: " << program.error_message << '\n';
        return 1;
    }

    std::cout << "JSON command file parsed successfully. command_count="
              << program.value.commands.size() << '\n';

    if (validate_only) {
        // 只校验 JSON 结构，不连接 SDK、不访问真实机械臂。
        // 调试新 JSON 时建议先跑这个模式。
        std::cout << "validate-only mode: no SDK connection or command execution.\n";
        return 0;
    }

    // 这里选择 RealMan 驱动。以后更换机械臂品牌，只需要替换这个具体实现，
    // JsonCommandRunner 和 JSON 格式不需要变。
    rm_control::drivers::realman::RealManRobotArm robot;
    rm_control::core::JsonCommandRunner runner(robot);
    auto result = runner.run(program.value, std::cout);
    if (!result.ok) {
        std::cerr << "Command execution failed: " << result.error_message << '\n';
        return 2;
    }

    std::cout << "\nAll JSON commands finished.\n";
    return 0;
}
