/*
 * slope_estimator.c — slow-adapting terrain slope estimator (Issue #600).
 *
 * First-order IIR low-pass filter on fused IMU pitch with tau = SLOPE_TAU_S (5 s).
 * The long time constant means fast balance corrections and transients are
 * ignored; only sustained pitch bias (i.e., genuine slope) accumulates.
 *
 * Estimate is clamped to ±SLOPE_MAX_DEG (15°) per the safety requirement.
 */

#include "slope_estimator.h"
#include "jlink.h"

/* ---- slope_estimator_init() ---- */
void slope_estimator_init(slope_estimator_t *se)
{
    se->estimate_deg  = 0.0f;
    se->enabled       = true;
    /* Initialize so first send_tlm call fires immediately */
    se->last_tlm_ms   = (uint32_t)(-(uint32_t)(1000u / (SLOPE_TLM_HZ > 0u ? SLOPE_TLM_HZ : 1u)));
}

/* ---- slope_estimator_reset() ---- */
void slope_estimator_reset(slope_estimator_t *se)
{
    se->estimate_deg = 0.0f;
}

/* ---- slope_estimator_update() ---- */
void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt)
{
    if (!se->enabled || dt <= 0.0f) return;

    /*
     * First-order IIR: alpha = dt / (tau + dt)
     * At 1 kHz (dt=0.001): alpha ≈ 0.0002  → time to ~63% ≈ 5 s
     * At 100 Hz (dt=0.01):  alpha ≈ 0.002   → time to ~63% ≈ 5 s (same tau)
     */
    float alpha = dt / (SLOPE_TAU_S + dt);
    float raw   = se->estimate_deg * (1.0f - alpha) + pitch_deg * alpha;

    /* Clamp to ±SLOPE_MAX_DEG */
    if (raw >  SLOPE_MAX_DEG) raw =  SLOPE_MAX_DEG;
    if (raw < -SLOPE_MAX_DEG) raw = -SLOPE_MAX_DEG;

    se->estimate_deg = raw;
}

/* ---- slope_estimator_get_deg() ---- */
float slope_estimator_get_deg(const slope_estimator_t *se)
{
    if (!se->enabled) return 0.0f;
    return se->estimate_deg;
}

/* ---- slope_estimator_set_enabled() ---- */
void slope_estimator_set_enabled(slope_estimator_t *se, bool en)
{
    se->enabled = en;
}

/* ---- slope_estimator_send_tlm() ---- */
void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms)
{
    if (SLOPE_TLM_HZ == 0u) return;

    uint32_t interval_ms = 1000u / SLOPE_TLM_HZ;
    if ((now_ms - se->last_tlm_ms) < interval_ms) return;
    /* Cast away const for timestamp update -- only mutable field */
    ((slope_estimator_t *)se)->last_tlm_ms = now_ms;

    jlink_tlm_slope_t tlm;
    /* Scale to ×100 for 0.01° resolution, clamped to int16 range */
    float scaled = se->estimate_deg * 100.0f;
    if (scaled >  32767.0f) scaled =  32767.0f;
    if (scaled < -32768.0f) scaled = -32768.0f;
    tlm.slope_x100 = (int16_t)scaled;
    tlm.active     = se->enabled ? 1u : 0u;
    tlm._pad       = 0u;

    jlink_send_slope_tlm(&tlm);
}