#[doc = include_str!("../README.md")]
use std::fmt::Debug;
use thiserror::Error;

pub mod ekf;
pub mod particle;

/// Errors that can occur during state estimation.
#[derive(Debug, Error)]
pub enum EstimatorError {
    /// The filter's internal math produced a matrix that can't be inverted.
    ///
    /// This usually means your measurement noise values are too small.
    /// Try increasing `measurement_noise_std` in your measurement model.
    #[error(
        "Could not compute the measurement update. \
         This usually means measurement noise is set too low. \
         Try increasing measurement_noise_std values."
    )]
    SingularInnovationCovariance,

    /// All particle weights dropped to zero.
    ///
    /// This happens when no particles match the measurements at all.
    /// Possible causes:
    /// - Initial particles are too far from the true state
    /// - Process noise is too low (particles aren't spreading enough)
    /// - Measurement model is too strict
    #[error(
        "All particle weights are zero — no particles match the measurements. \
         Try: (1) more particles, (2) wider initial distribution, \
         (3) more process noise, or (4) less strict likelihood function."
    )]
    ParticleDegeneracy,

    /// The filter has no particles.
    #[error("Particle filter has no particles")]
    NoParticles,

    /// A numerical computation produced NaN or infinity.
    #[error("Numerical instability: computation produced {0}")]
    NumericalInstability(String),
}

/// Result type for estimator operations.
pub type Result<T> = std::result::Result<T, EstimatorError>;

/// A state estimator that tracks something over time.
///
/// All estimators follow the same pattern:
/// 1. **Predict**: Use your model to guess where things will be
/// 2. **Update**: Use a measurement to correct your guess
/// 3. **Read**: Get the current best estimate
pub trait StateEstimator {
    /// What you're tracking (position, velocity, temperature, etc.)
    type State: Clone + Debug;

    /// What your sensors report
    type Measurement;

    /// Control inputs to your system (use `()` if there are none)
    type Control;

    /// Predict where the state will be after `dt` seconds.
    ///
    /// Call this once per time step, before you get a new measurement.
    /// This step rarely fails — it just propagates your model forward.
    fn predict(&mut self, control: &Self::Control, dt: f64);

    /// Update the estimate using a new sensor measurement.
    ///
    /// This corrects the prediction based on what you actually observed.
    ///
    /// # Errors
    ///
    /// Can fail if:
    /// - Measurement noise is set too low (EKF)
    /// - All particles have zero weight (Particle Filter)
    fn update(&mut self, measurement: &Self::Measurement) -> Result<()>;

    /// Get the current best estimate of the state.
    fn estimate(&self) -> Self::State;

    /// Convenience method: predict then update in one call.
    ///
    /// # Errors
    ///
    /// Returns an error if the update step fails.
    fn step(
        &mut self,
        control: &Self::Control,
        measurement: &Self::Measurement,
        dt: f64,
    ) -> Result<Self::State> {
        self.predict(control, dt);
        self.update(measurement)?;
        Ok(self.estimate())
    }
}

/// For estimators that can tell you how confident they are.
pub trait UncertaintyEstimator: StateEstimator {
    /// Get the standard deviation (uncertainty) for each state variable.
    ///
    /// Smaller values = more confident in the estimate.
    fn uncertainty(&self) -> Vec<f64>;
}

/// For particle-based estimators.
pub trait ParticleEstimator: StateEstimator {
    /// Iterate over all particles and their weights.
    fn particles(&self) -> impl Iterator<Item = (&Self::State, f64)>;

    /// Number of particles being used.
    fn num_particles(&self) -> usize;

    /// How "healthy" the particle distribution is (1.0 = perfect, 0.0 = degenerate).
    ///
    /// If this drops too low, the filter automatically resamples.
    fn particle_diversity(&self) -> f64;
}

#[cfg(test)]
mod tests;