#include "mocks/mock_dexterous_hand.h"
#include "mocks/mock_robot_arm.h"

#include "rm_control/core/force_spike_hand_guard.h"

#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::common::ForceData MakeWorkZeroForce(
    const std::array<float, 6U> &sample) {
    rm_control::common::ForceData data;
    data.work_zero = sample;
    return data;
}

rm_control::core::ForceSpikeHandGuardConfig GuardConfig() {
    rm_control::core::ForceSpikeHandGuardConfig config;
    config.detector.maximum_force_delta_newton = 5.0F;
    config.detector.maximum_torque_delta_newton_meter = 0.8F;
    config.open_hand_command =
        rm_control::core::BuildDefaultO6FiveFingerOpenCommand();
    return config;
}

void TestFirstSampleOnlyInitializesBaseline(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    robot.force_data_samples = {
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
    };
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());

    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(result.ok, "baseline force sample should succeed");
    context->Expect(result.value.detection.baseline_initialized,
                    "first guard sample should initialize detector baseline");
    context->Expect(!result.value.open_command_sent,
                    "baseline initialization must not command the hand");
    context->Expect(hand.command_calls == 0,
                    "hand must not move before a force spike exists");
}

void TestForceSpikeCommandsDefaultOpenPoseImmediately(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    robot.force_data_samples = {
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({0.0F, 5.2F, 0.0F, 0.0F, 0.0F, 0.0F}),
    };
    rm_control::tests::MockDexterousHand hand;
    const auto expected_open_command =
        rm_control::core::BuildDefaultO6FiveFingerOpenCommand();
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());

    const auto first = guard.CheckForceOnceAndOpenHandOnSpike();
    const auto second = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(first.ok && second.ok,
                    "baseline and spike guard steps should succeed");
    context->Expect(second.value.detection.spike_detected,
                    "second sample should detect the force spike");
    context->Expect(second.value.open_command_sent,
                    "force spike should command O6 open in the same guard step");
    context->Expect(hand.readiness_calls == 0,
                    "immediate open command should not add a readiness round trip");
    context->Expect(hand.command_calls == 1,
                    "force spike should send exactly one hand command");
    context->Expect(hand.last_motion_command.target_positions ==
                        expected_open_command.target_positions,
                    "force spike should send the configured five-finger open pose");
    context->Expect(hand.last_motion_command.joint_speed_commands ==
                        expected_open_command.joint_speed_commands,
                    "force spike should send the configured open speed commands");
    context->Expect(hand.last_motion_command.joint_torque_commands ==
                        expected_open_command.joint_torque_commands,
                    "force spike should send the configured open torque commands");
}

void TestLatchSuppressesRepeatedOpenUntilReset(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    robot.force_data_samples = {
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({6.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
    };
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());

    (void)guard.CheckForceOnceAndOpenHandOnSpike();
    const auto first_spike = guard.CheckForceOnceAndOpenHandOnSpike();
    (void)guard.CheckForceOnceAndOpenHandOnSpike();
    const auto latched_spike = guard.CheckForceOnceAndOpenHandOnSpike();
    guard.ResetOpenCommandLatch();
    (void)guard.CheckForceOnceAndOpenHandOnSpike();
    const auto reset_spike = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(first_spike.ok && first_spike.value.open_command_sent,
                    "first spike should command open hand");
    context->Expect(latched_spike.ok &&
                        latched_spike.value.open_command_suppressed_by_latch,
                    "latched guard should suppress repeated open command");
    context->Expect(reset_spike.ok && reset_spike.value.open_command_sent,
                    "reset latch should allow a later spike to command open again");
    context->Expect(hand.command_calls == 2,
                    "guard should send one command before reset and one after reset");
}

void TestForceReadFailurePreventsOpenCommand(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    robot.force_data_result =
        rm_control::common::Result<rm_control::common::ForceData>::failure(
            "mock force read failed");
    rm_control::tests::MockDexterousHand hand;
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());

    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(!result.ok, "force read failure should fail the guard step");
    context->Expect(hand.command_calls == 0,
                    "force read failure must not command the hand");
}

void TestInvalidConfigFailsBeforeReadingForce(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    rm_control::tests::MockDexterousHand hand;
    auto config = GuardConfig();
    config.detector.maximum_force_delta_newton = 0.0F;
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, config);

    const auto result = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(!result.ok, "invalid force threshold should fail the guard step");
    context->Expect(robot.force_data_calls == 0,
                    "invalid config must be rejected before reading hardware state");
    context->Expect(hand.command_calls == 0,
                    "invalid config must not command the hand");
}

void TestHandCommandFailureIsReturned(TestContext *context) {
    rm_control::tests::MockRobotArm robot;
    robot.force_data_samples = {
        MakeWorkZeroForce({0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F}),
        MakeWorkZeroForce({0.0F, 0.0F, 5.5F, 0.0F, 0.0F, 0.0F}),
    };
    rm_control::tests::MockDexterousHand hand;
    hand.command_result =
        rm_control::common::Result<void>::failure("mock hand command failed");
    rm_control::core::ForceSpikeHandGuard guard(robot, hand, GuardConfig());

    const auto first = guard.CheckForceOnceAndOpenHandOnSpike();
    const auto second = guard.CheckForceOnceAndOpenHandOnSpike();

    context->Expect(first.ok, "baseline guard step should succeed");
    context->Expect(!second.ok, "failed O6 open command should fail the guard step");
    context->Expect(hand.command_calls == 1,
                    "force spike should attempt exactly one O6 open command");
}

} // namespace

int main() {
    TestContext context;
    TestFirstSampleOnlyInitializesBaseline(&context);
    TestForceSpikeCommandsDefaultOpenPoseImmediately(&context);
    TestLatchSuppressesRepeatedOpenUntilReset(&context);
    TestForceReadFailurePreventsOpenCommand(&context);
    TestInvalidConfigFailsBeforeReadingForce(&context);
    TestHandCommandFailureIsReturned(&context);

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