#pragma once

#include "rm_control/common/result.h"
#include "rm_control/common/robot_types.h"

#include <array>
#include <cstddef>

namespace rm_control::modules::force_safety {

enum class ForceDataSignal {
    Raw,
    Zero,
    WorkZero,
    ToolZero,
};

struct ForceSpikeDetectorConfig {
    ForceDataSignal signal = ForceDataSignal::WorkZero;
    float maximum_force_delta_newton = 5.0F;
    float maximum_torque_delta_newton_meter = 0.8F;
};

struct ForceSpikeDetectorState {
    bool has_previous_sample = false;
    std::array<float, 6U> previous_sample{};
};

struct ForceSpikeDetectionReport {
    bool baseline_initialized = false;
    bool spike_detected = false;
    std::size_t trigger_axis = 0U;
    float measured_delta = 0.0F;
    float configured_limit = 0.0F;
    std::array<float, 6U> previous_sample{};
    std::array<float, 6U> current_sample{};
};

common::Result<void> ValidateForceSpikeDetectorConfig(
    const ForceSpikeDetectorConfig &config);

std::array<float, 6U> SelectForceSignal(
    const common::ForceData &force_data,
    ForceDataSignal signal);

common::Result<ForceSpikeDetectionReport> UpdateForceSpikeDetector(
    ForceSpikeDetectorState *state,
    const std::array<float, 6U> &current_sample,
    const ForceSpikeDetectorConfig &config);

} // namespace rm_control::modules::force_safety
