#include "rm_control/config/drag_teach_safety_config.h"

#include <filesystem>
#include <fstream>
#include <iostream>
#include <string>

#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif

namespace {

struct TestContext {
    int failures = 0;

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

std::filesystem::path WriteConfig(const std::string &name, const std::string &text) {
    const std::filesystem::path directory =
        std::filesystem::path(RM_TEST_OUTPUT_DIR) / "drag_teach_safety_config";
    std::filesystem::create_directories(directory);
    const std::filesystem::path path = directory / name;
    std::ofstream output(path, std::ios::trunc);
    output << text;
    return path;
}

std::string ValidConfigText() {
    return R"({
  "hardware_execution_enabled": false,
  "emergency_stop_verified": false,
  "safety_limits_loaded": true,
  "workspace_and_virtual_wall_verified": true,
  "native_replay_without_speed_control_accepted": false,
  "trajectory_sample_period_confirmed": false,
  "vendor_raw_units_per_degree_confirmed": false,
  "max_speed_percent": 20,
  "vendor_raw_units_per_degree": 1000,
  "maximum_origin_move_delta_deg": 30.0,
  "max_joint_jump_deg": 5.0,
  "max_joint_velocity_deg_per_second": 30.0,
  "max_joint_acceleration_deg_per_second_squared": 100.0,
  "maximum_absolute_force_newton": 100.0,
  "maximum_absolute_torque_newton_meter": 20.0,
  "maximum_o6_speed_command": 64,
  "maximum_o6_torque_command": 80
})";
}

void TestLoadsExplicitDisabledConfiguration(TestContext *context) {
    const auto path = WriteConfig("valid.json", ValidConfigText());
    const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
    context->Expect(result.ok, "valid safety config should load");
    if (!result.ok) {
        return;
    }
    context->Expect(!result.value.hardware_execution_enabled,
                    "hardware execution must remain explicitly disabled");
    context->Expect(result.value.max_speed_percent == 20, "max speed should be parsed");
    context->Expect(result.value.vendor_raw_units_per_degree == 1000,
                    "vendor raw-units-per-degree ratio should be parsed");
    context->Expect(!result.value.trajectory_sample_period_confirmed &&
                        !result.value.vendor_raw_units_per_degree_confirmed,
                    "unconfirmed vendor timing and units must remain false");
    context->Expect(result.value.maximum_origin_move_delta_deg == 30.0F,
                    "maximum origin move delta should be parsed");
    context->Expect(result.value.maximum_o6_speed_command == 64U,
                    "maximum O6 speed command should be parsed");
    context->Expect(result.value.maximum_o6_torque_command == 80U,
                    "maximum O6 torque command should be parsed");
}

void TestRejectsMissingRequiredField(TestContext *context) {
    const auto path = WriteConfig(
        "missing.json",
        R"({"hardware_execution_enabled": false})");
    const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
    context->Expect(!result.ok, "missing safety fields must be rejected");
}

void TestRejectsInvalidLimit(TestContext *context) {
    std::string text = ValidConfigText();
    const std::string original = "\"max_speed_percent\": 20";
    text.replace(text.find(original), original.size(), "\"max_speed_percent\": 0");
    const auto path = WriteConfig("invalid_limit.json", text);
    const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
    context->Expect(!result.ok, "zero maximum speed must be rejected");
}

void TestRejectsO6CommandLimitAboveProtocolRange(TestContext *context) {
    std::string text = ValidConfigText();
    const std::string original = "\"maximum_o6_torque_command\": 80";
    text.replace(text.find(original), original.size(),
                 "\"maximum_o6_torque_command\": 256");
    const auto path = WriteConfig("invalid_o6_limit.json", text);
    const auto result = rm_control::config::LoadDragTeachSafetyConfig(path);
    context->Expect(!result.ok, "O6 torque command limit above 255 must be rejected");
}

} // namespace

int main() {
    TestContext context;
    TestLoadsExplicitDisabledConfiguration(&context);
    TestRejectsMissingRequiredField(&context);
    TestRejectsInvalidLimit(&context);
    TestRejectsO6CommandLimitAboveProtocolRange(&context);

    if (context.failures == 0) {
        std::cout << "[PASS] rm_drag_teach_safety_config_tests\n";
        return 0;
    }
    std::cerr << context.failures << " test(s) failed\n";
    return 1;
}
