#include "rm_control/modules/force_safety/force_spike_detector.h"

#include <cmath>
#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::modules::force_safety::ForceSpikeDetectorConfig DetectorConfig() {
    rm_control::modules::force_safety::ForceSpikeDetectorConfig config;
    config.maximum_force_delta_newton = 5.0F;
    config.maximum_torque_delta_newton_meter = 0.8F;
    return config;
}

void TestFirstSampleInitializesBaselineWithoutSpike(TestContext *context) {
    rm_control::modules::force_safety::ForceSpikeDetectorState state;
    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {1.0F, 2.0F, 3.0F, 0.1F, 0.2F, 0.3F}, DetectorConfig());

    context->Expect(result.ok, "first force sample should be accepted");
    context->Expect(result.value.baseline_initialized,
                    "first force sample should initialize the baseline");
    context->Expect(!result.value.spike_detected,
                    "first force sample must not trigger a spike");
    context->Expect(state.has_previous_sample,
                    "detector should remember the first sample");
}

void TestForceAxisDeltaAtThresholdTriggersSpike(TestContext *context) {
    rm_control::modules::force_safety::ForceSpikeDetectorState state;
    const auto config = DetectorConfig();
    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);

    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {0.0F, 5.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);

    context->Expect(result.ok, "force spike detector should accept finite sample");
    context->Expect(result.value.spike_detected,
                    "force delta equal to threshold should trigger");
    context->Expect(result.value.trigger_axis == 1U,
                    "force spike should report the triggering force axis");
    context->Expect(result.value.configured_limit == config.maximum_force_delta_newton,
                    "force spike should report the force threshold");
}

void TestTorqueAxisUsesTorqueThreshold(TestContext *context) {
    rm_control::modules::force_safety::ForceSpikeDetectorState state;
    const auto config = DetectorConfig();
    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);

    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.9F, 0.0F}, config);

    context->Expect(result.ok, "torque spike detector should accept finite sample");
    context->Expect(result.value.spike_detected,
                    "torque delta above torque threshold should trigger");
    context->Expect(result.value.trigger_axis == 4U,
                    "torque spike should report the triggering torque axis");
    context->Expect(result.value.configured_limit ==
                        config.maximum_torque_delta_newton_meter,
                    "torque spike should report the torque threshold");
}

void TestBelowThresholdUpdatesBaselineWithoutSpike(TestContext *context) {
    rm_control::modules::force_safety::ForceSpikeDetectorState state;
    const auto config = DetectorConfig();
    (void)rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}, config);

    const auto result = rm_control::modules::force_safety::UpdateForceSpikeDetector(
        &state, {4.9F, 0.0F, 0.0F, 0.0F, 0.7F, 0.0F}, config);

    context->Expect(result.ok, "below-threshold sample should be accepted");
    context->Expect(!result.value.spike_detected,
                    "below-threshold force and torque should not trigger");
    context->Expect(state.previous_sample[0] == 4.9F &&
                        state.previous_sample[4] == 0.7F,
                    "detector should advance the baseline after every valid sample");
}

void TestInvalidConfigAndNonFiniteSampleFailClosed(TestContext *context) {
    auto invalid_config = DetectorConfig();
    invalid_config.maximum_force_delta_newton = 0.0F;
    rm_control::modules::force_safety::ForceSpikeDetectorState state;
    const auto invalid_config_result =
        rm_control::modules::force_safety::UpdateForceSpikeDetector(
            &state, {}, invalid_config);
    context->Expect(!invalid_config_result.ok,
                    "zero force threshold must be rejected");

    const auto non_finite_sample =
        rm_control::modules::force_safety::UpdateForceSpikeDetector(
            &state,
            {0.0F, std::nanf(""), 0.0F, 0.0F, 0.0F, 0.0F},
            DetectorConfig());
    context->Expect(!non_finite_sample.ok,
                    "non-finite force sample must be rejected");
}

void TestSelectForceSignalKeepsConfiguredForceFrame(TestContext *context) {
    rm_control::common::ForceData force_data;
    force_data.raw = {1.0F, 1.0F, 1.0F, 1.0F, 1.0F, 1.0F};
    force_data.zero = {2.0F, 2.0F, 2.0F, 2.0F, 2.0F, 2.0F};
    force_data.work_zero = {3.0F, 3.0F, 3.0F, 3.0F, 3.0F, 3.0F};
    force_data.tool_zero = {4.0F, 4.0F, 4.0F, 4.0F, 4.0F, 4.0F};

    const auto selected = rm_control::modules::force_safety::SelectForceSignal(
        force_data, rm_control::modules::force_safety::ForceDataSignal::ToolZero);

    context->Expect(selected[0] == 4.0F && selected[5] == 4.0F,
                    "force detector should use the configured force data frame");
}

} // namespace

int main() {
    TestContext context;
    TestFirstSampleInitializesBaselineWithoutSpike(&context);
    TestForceAxisDeltaAtThresholdTriggersSpike(&context);
    TestTorqueAxisUsesTorqueThreshold(&context);
    TestBelowThresholdUpdatesBaselineWithoutSpike(&context);
    TestInvalidConfigAndNonFiniteSampleFailClosed(&context);
    TestSelectForceSignalKeepsConfiguredForceFrame(&context);

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