use core::f32::consts::PI;

/// Normalize angle to [-PI, PI]
fn wrap_angle(angle: f32) -> f32 {
    let mut a = angle;
    while a >  PI { a -= 2.0*PI; }
    while a < -PI { a += 2.0*PI; }
    a
}

/// Heading estimator using gyro yaw rate
#[derive(Debug, Clone, Copy, Default)]
pub struct HeadingEstimator {
    heading: f32, // radians
}

impl HeadingEstimator {
    /// Integrate yaw rate (rad/s) over dt (s)
    pub fn update(&mut self, yaw_rate: f32, dt: f32) {
        self.heading = wrap_angle(self.heading + yaw_rate * dt);
    }

    /// Correct drift using an external absolute heading (e.g. magnetometer)
    pub fn correct(&mut self, measured_heading: f32, alpha: f32) {
        // alpha ∈ [0,1]: 0 = ignore measurement, 1 = full trust measurement
        let err = wrap_angle(measured_heading - self.heading);
        self.heading = wrap_angle(self.heading + alpha * err);
    }

    pub fn heading(&self) -> f32 {
        self.heading
    }
}
