#include "rm_control/core/hardware_execution_gate.h"

#include <array>
#include <iostream>
#include <string>

namespace {

struct TestContext {
    int failures = 0;

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

rm_control::core::HardwareExecutionGateInput MakeAllowedInput() {
    rm_control::core::HardwareExecutionGateInput input;
    input.live_requested = true;
    input.hardware_execution_enabled = true;
    input.arm_connected = true;
    input.hand_connected = true;
    input.emergency_stop_verified = true;
    input.safety_limits_loaded = true;
    input.operator_confirmed = true;
    input.trajectory_validated = true;
    input.trajectory_sample_period_confirmed = true;
    input.vendor_raw_units_per_degree_confirmed = true;
    input.workspace_and_virtual_wall_verified = true;
    input.configured_max_speed_percent = 20;
    input.native_replay_without_speed_control_accepted = true;
    input.native_replay_speed_within_config_verified = true;
    return input;
}

void TestAllowsExecutionWhenEveryBaseConditionIsSatisfied(TestContext *context) {
    const auto decision =
        rm_control::core::EvaluateHardwareExecutionGate(MakeAllowedInput());
    context->Expect(decision.execution_allowed,
                    "complete base safety input should allow execution");
    context->Expect(decision.rejection_reasons.empty(),
                    "allowed decision should contain no rejection");
}

void TestRejectsEachMissingBaseCondition(TestContext *context) {
    using Input = rm_control::core::HardwareExecutionGateInput;
    constexpr std::array<bool Input::*, 9> kRequiredConditions = {
        &Input::live_requested,
        &Input::hardware_execution_enabled,
        &Input::arm_connected,
        &Input::hand_connected,
        &Input::emergency_stop_verified,
        &Input::safety_limits_loaded,
        &Input::operator_confirmed,
        &Input::trajectory_validated,
        &Input::workspace_and_virtual_wall_verified,
    };

    for (const auto required_condition : kRequiredConditions) {
        auto input = MakeAllowedInput();
        input.*required_condition = false;
        const auto decision = rm_control::core::EvaluateHardwareExecutionGate(input);
        context->Expect(!decision.execution_allowed,
                        "each missing base condition must reject execution");
        context->Expect(!decision.rejection_reasons.empty(),
                        "a missing base condition must explain the rejection");
    }
}

void TestRejectsUnconfirmedVendorMetadataWhenRequired(TestContext *context) {
    rm_control::core::HardwareExecutionGateRequirements requirements;
    requirements.require_confirmed_trajectory_sample_period = true;
    requirements.require_confirmed_vendor_raw_units_per_degree = true;

    auto input = MakeAllowedInput();
    input.trajectory_sample_period_confirmed = false;
    auto decision =
        rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
    context->Expect(!decision.execution_allowed,
                    "required unconfirmed sample period must reject playback");

    input = MakeAllowedInput();
    input.vendor_raw_units_per_degree_confirmed = false;
    decision = rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
    context->Expect(!decision.execution_allowed,
                    "required unconfirmed raw-unit ratio must reject playback");
}

void TestRejectsNativeReplayWithoutExplicitAcceptance(TestContext *context) {
    auto input = MakeAllowedInput();
    input.native_replay_without_speed_control_accepted = false;
    const rm_control::core::HardwareExecutionGateRequirements requirements{
        0, true};

    const auto decision =
        rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
    context->Expect(!decision.execution_allowed,
                    "native replay must require acceptance of missing speed control");
}

void TestRejectsNativeReplayWithoutIndependentSpeedVerification(
    TestContext *context) {
    auto input = MakeAllowedInput();
    input.native_replay_speed_within_config_verified = false;
    const rm_control::core::HardwareExecutionGateRequirements requirements{
        0, true, true};
    const auto decision =
        rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
    context->Expect(!decision.execution_allowed,
                    "native replay must verify actual speed against the configured limit");
}

void TestAllowsNativeOperationsWhenTheirSpecificRequirementsAreSatisfied(
    TestContext *context) {
    const auto input = MakeAllowedInput();
    const rm_control::core::HardwareExecutionGateRequirements origin_requirements{
        rm_control::core::kNativeTrajectoryOriginRequiredSpeedPercent, false};
    const rm_control::core::HardwareExecutionGateRequirements replay_requirements{0, true};

    const auto origin_decision =
        rm_control::core::EvaluateHardwareExecutionGate(input, origin_requirements);
    const auto replay_decision =
        rm_control::core::EvaluateHardwareExecutionGate(input, replay_requirements);
    context->Expect(origin_decision.execution_allowed,
                    "native origin motion should pass at the configured 20 percent limit");
    context->Expect(replay_decision.execution_allowed,
                    "native replay should pass after accepting its missing speed control");
}

void TestRejectsNativeOriginWhenRequiredSpeedExceedsConfiguredMaximum(
    TestContext *context) {
    auto input = MakeAllowedInput();
    input.configured_max_speed_percent = 10;
    const rm_control::core::HardwareExecutionGateRequirements requirements{
        rm_control::core::kNativeTrajectoryOriginRequiredSpeedPercent, false};

    const auto result =
        rm_control::core::RequireHardwareExecutionPermission(input, requirements);
    context->Expect(!result.ok,
                    "native 20 percent origin motion must fail below configured maximum");
    context->Expect(result.error_message.find("exceeds configured maximum") !=
                        std::string::npos,
                    "speed rejection should include actionable context");
}

void TestRejectsInvalidSpeedRanges(TestContext *context) {
    auto input = MakeAllowedInput();
    input.configured_max_speed_percent = 101;
    auto decision = rm_control::core::EvaluateHardwareExecutionGate(input);
    context->Expect(!decision.execution_allowed,
                    "configured speed above 100 must be rejected");

    input = MakeAllowedInput();
    const rm_control::core::HardwareExecutionGateRequirements requirements{-1, false};
    decision = rm_control::core::EvaluateHardwareExecutionGate(input, requirements);
    context->Expect(!decision.execution_allowed,
                    "negative required speed must be rejected");
}

} // namespace

int main() {
    TestContext context;
    TestAllowsExecutionWhenEveryBaseConditionIsSatisfied(&context);
    TestRejectsEachMissingBaseCondition(&context);
    TestRejectsUnconfirmedVendorMetadataWhenRequired(&context);
    TestRejectsNativeReplayWithoutExplicitAcceptance(&context);
    TestRejectsNativeReplayWithoutIndependentSpeedVerification(&context);
    TestAllowsNativeOperationsWhenTheirSpecificRequirementsAreSatisfied(&context);
    TestRejectsNativeOriginWhenRequiredSpeedExceedsConfiguredMaximum(&context);
    TestRejectsInvalidSpeedRanges(&context);

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