#ifndef SLOPE_ESTIMATOR_H
#define SLOPE_ESTIMATOR_H

#include <stdint.h>
#include <stdbool.h>

/*
 * slope_estimator — slow-adapting terrain slope estimator for Issue #600.
 *
 * On a slope the robot must lean slightly into the hill to stay balanced.
 * The IMU pitch reading therefore includes both the robot's balance offset
 * and the ground incline.  This module decouples the two by tracking the
 * slowly-changing DC component of the pitch signal with a first-order IIR
 * low-pass filter (time constant SLOPE_TAU_S = 5 s).
 *
 * HOW IT WORKS:
 *   Every call to slope_estimator_update(pitch_deg, dt):
 *
 *     alpha    = dt / (SLOPE_TAU_S + dt)       // ≈ 0.0002 at 1 kHz
 *     raw      = slope * (1 - alpha) + pitch * alpha
 *     slope    = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG)
 *
 *   The IIR converges to the steady-state pitch in ~5 s.  Fast tilt
 *   transients (balance corrections, steps, bumps) are attenuated by
 *   the long time constant and do not corrupt the estimate.
 *
 * INTEGRATION IN BALANCE PID:
 *   Subtract slope_estimate from the measured pitch before computing
 *   the PID error so the controller balances around the slope surface
 *   rather than absolute vertical:
 *
 *     tilt_corrected = pitch_deg - slope_estimate_deg
 *     error = setpoint - tilt_corrected
 *
 *   This is equivalent to continuously adjusting the balance setpoint
 *   to track the incline.
 *
 * SAFETY:
 *   - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from
 *     extreme falls being mistaken for genuine slopes.
 *   - slope_estimator_reset() zeroes the state; call on disarm or after
 *     a tilt fault so re-arming starts fresh.
 *
 * TELEMETRY:
 *   JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz):
 *     jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad }
 */

/* ---- Configuration ---- */
#define SLOPE_TAU_S         5.0f    /* IIR time constant (seconds)          */
#define SLOPE_MAX_DEG      15.0f    /* Maximum estimate magnitude (degrees) */
#define SLOPE_TLM_HZ        1u      /* JLINK_TLM_SLOPE publish rate (Hz)    */

/* ---- State ---- */
typedef struct {
    float    estimate_deg;   /* current slope estimate (degrees, + = nose-up) */
    bool     enabled;        /* compensation on/off; off = estimate frozen     */
    uint32_t last_tlm_ms;   /* timestamp of last TLM transmission             */
} slope_estimator_t;

/* ---- API ---- */

/*
 * slope_estimator_init(se) — zero state, enable estimation.
 * Call once during system init.
 */
void slope_estimator_init(slope_estimator_t *se);

/*
 * slope_estimator_reset(se) — zero estimate without changing enabled flag.
 * Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm.
 */
void slope_estimator_reset(slope_estimator_t *se);

/*
 * slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter.
 *   pitch_deg : current fused pitch angle from IMU (degrees)
 *   dt        : loop interval (seconds)
 * No-op if se->enabled == false or dt <= 0.
 */
void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt);

/*
 * slope_estimator_get_deg(se) — return current estimate (degrees).
 * Returns 0 if disabled.
 */
float slope_estimator_get_deg(const slope_estimator_t *se);

/*
 * slope_estimator_set_enabled(se, en) — enable or disable compensation.
 * Disabling freezes the estimate at its current value.
 */
void slope_estimator_set_enabled(slope_estimator_t *se, bool en);

/*
 * slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88)
 * frame to Jetson.  Rate-limited to SLOPE_TLM_HZ; safe to call every tick.
 */
void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms);

#endif /* SLOPE_ESTIMATOR_H */