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

#include <cmath>
#include <string>

namespace rm_control::modules::force_safety {
namespace {

constexpr std::size_t kFirstTorqueAxis = 3U;

bool IsFinitePositive(float value) {
    return std::isfinite(value) && value > 0.0F;
}

bool ForceSampleIsFinite(const std::array<float, 6U> &sample) {
    for (const float value : sample) {
        if (!std::isfinite(value)) {
            return false;
        }
    }
    return true;
}

float LimitForAxis(std::size_t axis, const ForceSpikeDetectorConfig &config) {
    return axis < kFirstTorqueAxis
        ? config.maximum_force_delta_newton
        : config.maximum_torque_delta_newton_meter;
}

} // namespace

common::Result<void> ValidateForceSpikeDetectorConfig(
    const ForceSpikeDetectorConfig &config) {
    if (!IsFinitePositive(config.maximum_force_delta_newton)) {
        return common::Result<void>::failure(
            "force spike detector force delta must be finite and positive");
    }
    if (!IsFinitePositive(config.maximum_torque_delta_newton_meter)) {
        return common::Result<void>::failure(
            "force spike detector torque delta must be finite and positive");
    }
    return common::Result<void>::success();
}

std::array<float, 6U> SelectForceSignal(
    const common::ForceData &force_data,
    ForceDataSignal signal) {
    switch (signal) {
    case ForceDataSignal::Raw:
        return force_data.raw;
    case ForceDataSignal::Zero:
        return force_data.zero;
    case ForceDataSignal::WorkZero:
        return force_data.work_zero;
    case ForceDataSignal::ToolZero:
        return force_data.tool_zero;
    }
    return force_data.work_zero;
}

common::Result<ForceSpikeDetectionReport> UpdateForceSpikeDetector(
    ForceSpikeDetectorState *state,
    const std::array<float, 6U> &current_sample,
    const ForceSpikeDetectorConfig &config) {
    if (state == nullptr) {
        return common::Result<ForceSpikeDetectionReport>::failure(
            "force spike detector state must not be null");
    }
    const auto config_check = ValidateForceSpikeDetectorConfig(config);
    if (!config_check.ok) {
        return common::Result<ForceSpikeDetectionReport>::failure(
            config_check.error_message);
    }
    if (!ForceSampleIsFinite(current_sample)) {
        return common::Result<ForceSpikeDetectionReport>::failure(
            "force spike detector sample contains a non-finite value");
    }

    ForceSpikeDetectionReport report;
    report.current_sample = current_sample;
    if (!state->has_previous_sample) {
        state->previous_sample = current_sample;
        state->has_previous_sample = true;
        report.baseline_initialized = true;
        report.previous_sample = current_sample;
        return common::Result<ForceSpikeDetectionReport>::success(report);
    }

    report.previous_sample = state->previous_sample;
    for (std::size_t axis = 0U; axis < current_sample.size(); ++axis) {
        const float delta = std::fabs(current_sample[axis] - state->previous_sample[axis]);
        const float limit = LimitForAxis(axis, config);
        if (delta >= limit) {
            report.spike_detected = true;
            report.trigger_axis = axis;
            report.measured_delta = delta;
            report.configured_limit = limit;
            break;
        }
    }

    state->previous_sample = current_sample;
    return common::Result<ForceSpikeDetectionReport>::success(report);
}

} // namespace rm_control::modules::force_safety
