#pragma once

#include "rm_control/common/result.h"
#include "rm_control/interfaces/dexterous_hand.h"
#include "rm_control/interfaces/robot_arm.h"
#include "rm_control/modules/force_safety/force_spike_detector.h"

namespace rm_control::core {

interfaces::O6MotionCommand BuildDefaultO6FiveFingerOpenCommand();

struct ForceSpikeHandGuardConfig {
    modules::force_safety::ForceSpikeDetectorConfig detector;
    interfaces::O6MotionCommand open_hand_command =
        BuildDefaultO6FiveFingerOpenCommand();
    bool latch_after_successful_open_command = true;
};

struct ForceSpikeHandGuardStepReport {
    modules::force_safety::ForceSpikeDetectionReport detection;
    bool open_command_sent = false;
    bool open_command_suppressed_by_latch = false;
};

common::Result<void> ValidateForceSpikeHandGuardConfig(
    const ForceSpikeHandGuardConfig &config);

class ForceSpikeHandGuard final {
public:
    ForceSpikeHandGuard(interfaces::IRobotArm &robot,
                        interfaces::IDexterousHand &hand,
                        ForceSpikeHandGuardConfig config) noexcept;

    common::Result<ForceSpikeHandGuardStepReport>
    CheckForceOnceAndOpenHandOnSpike();

    void ResetOpenCommandLatch() noexcept;

private:
    interfaces::IRobotArm &robot_;
    interfaces::IDexterousHand &hand_;
    ForceSpikeHandGuardConfig config_;
    modules::force_safety::ForceSpikeDetectorState detector_state_;
    bool open_command_latched_ = false;
};

} // namespace rm_control::core
