#include "rm_control/config/json_command.h"

#include <iostream>
#include <string>

namespace {

int g_failures = 0;

void expect(bool condition, const std::string &message) {
    // 这里不用 gtest，保持 RM 子工程测试零外部依赖，方便离线编译。
    if (!condition) {
        ++g_failures;
        std::cerr << "[FAIL] " << message << '\n';
    }
}

void testParseReadonlyProgram() {
    // 只读命令必须能在不打开真实硬件的情况下完成解析。
    const std::string text = R"json(
{
  "connection": {
    "ip": "192.168.1.18",
    "port": 8080,
    "timeout_ms": 1000
  },
  "safety": {
    "dry_run": true,
    "allow_motion": false,
    "max_speed_percent": 20,
    "max_joint_delta_deg": 5
  },
  "commands": [
    {"type": "get_robot_info"},
    {"type": "get_current_arm_state"},
    {"type": "get_force_data"},
    {"type": "sleep_ms", "duration_ms": 10}
  ]
}
)json";

    auto program = rm_control::config::parseCommandProgramText(text);
    expect(program.ok, "read-only JSON should parse");
    if (!program.ok) {
        return;
    }
    expect(program.value.connection.ip == "192.168.1.18", "connection IP parsed");
    expect(program.value.commands.size() == 4, "command count parsed");
    expect(program.value.safety.dry_run, "dry_run parsed");
    expect(!program.value.safety.allow_motion, "allow_motion parsed");
}

void testParseMoveJDeltaProgram() {
    // 相对关节命令是人工调试最常用格式，必须保证 delta 和速度能正确解析。
    const std::string text = R"json(
{
  "commands": [
    {
      "type": "movej_delta",
      "name": "small J2 test",
      "delta_deg": [0, 1, 0, 0, 0, 0, 0],
      "speed_percent": 5,
      "blocking": true
    }
  ]
}
)json";

    auto program = rm_control::config::parseCommandProgramText(text);
    expect(program.ok, "movej_delta JSON should parse");
    if (!program.ok) {
        return;
    }
    expect(program.value.commands[0].joint_delta_deg.size() == 7, "delta size parsed");
    expect(program.value.commands[0].motion.speed_percent == 5, "speed parsed");
}

void testParseMoveJDeltaStepsProgram() {
    // 阶梯运动用于把较大总幅度拆成小步，核心字段是 steps 和 step_sleep_ms。
    const std::string text = R"json(
{
  "commands": [
    {
      "type": "movej_delta_steps",
      "name": "visible J4 test",
      "delta_deg": [0, 0, 0, 20, 0, 0, 0],
      "steps": 20,
      "step_sleep_ms": 20,
      "speed_percent": 20,
      "blocking": true
    }
  ]
}
)json";

    auto program = rm_control::config::parseCommandProgramText(text);
    expect(program.ok, "movej_delta_steps JSON should parse");
    if (!program.ok) {
        return;
    }
    expect(program.value.commands[0].steps == 20, "steps parsed");
    expect(program.value.commands[0].step_sleep_ms == 20, "step sleep parsed");
}

void testRejectInvalidProgram() {
    // 缺少必要字段时必须拒绝，避免不完整 JSON 被发送到真实机械臂。
    const std::string text = R"json({"commands":[{"type":"movej"}]})json";
    auto program = rm_control::config::parseCommandProgramText(text);
    expect(!program.ok, "movej without joints_deg should be rejected");
}

} // namespace

int main() {
    testParseReadonlyProgram();
    testParseMoveJDeltaProgram();
    testParseMoveJDeltaStepsProgram();
    testRejectInvalidProgram();

    if (g_failures == 0) {
        std::cout << "[PASS] rm_json_command_tests\n";
        return 0;
    }

    std::cerr << g_failures << " test(s) failed\n";
    return 1;
}
