//! Operation of a Bayesian state estimator in a range, angle observation example.
//!
//! The numerically stable [`UDState`] is used for estimation.
//!
//! The x,y cartesian position is estimated and range and angle of the position from the origin is observed.

use std::ops::Mul;
use nalgebra::{Matrix2, Vector2};

use bayes_estimate::estimators::ud::UDState;
use bayes_estimate::models::{KalmanEstimator, KalmanState};
use bayes_estimate::noise::{CoupledNoise, UncorrelatedNoise};

fn main() {
    // Initial position estimate with no uncertainty
    let init = KalmanState {
        x: Vector2::new(11., 1.), // initially at 11, 1
        X: Matrix2::zeros(),  // with no uncertainty
    };

    // Initialise the UDState estimate
    let mut estimate = UDState::try_from(init).unwrap();
    let estimate_init = estimate.kalman_state().unwrap();
    println!("Initial x{:.1} X{:.2}", estimate_init.x, estimate_init.X);

    // Make a position prediction adding uncertainty
    let my_predict_model = Matrix2::identity();
    let my_predict_noise = CoupledNoise::from_uncorrelated(UncorrelatedNoise {
        q: Vector2::new(sqr(1.), sqr(1.)),
    });
    let predicted_x = my_predict_model * estimate.x;
    estimate.predict(&my_predict_model, &predicted_x, &my_predict_noise).unwrap();
    let estimate_predict = estimate.kalman_state().unwrap();
    println!("Predict x{:.1} X{:.2}", estimate_predict.x, estimate_predict.X);

    // Build the linearised range,angle observation model using predicted position
    let xp = estimate_predict.x[0];
    let yp = estimate_predict.x[1];
    let rp2: f64 = sqr(xp) + sqr(yp);
    let rp = rp2.sqrt();
    let ap = yp.atan2(xp);
    let my_observe_model = Matrix2::new(
         xp / rp,  yp / rp,
        -yp / rp2, xp / rp2);

    // Make a range, angle observation that we appear to be at 10,0 with added uncertainty
    let z = Vector2::new(10., 0_f64.to_radians());
    let my_observe_noise = UncorrelatedNoise {
        q: Vector2::new(sqr(0.5), sqr(2_f64.to_radians())),
    };
    // Make the position observation using the difference of what we observe to predicted observation
    let innovation = z - Vector2::new(rp, ap);
    estimate.observe_innovation(&innovation, &my_observe_model, &my_observe_noise).unwrap();

    // The estimated state and covariance
    let estimate_state = estimate.kalman_state().unwrap();
    println!("Observe x{:.1} X{:.2}", estimate_state.x, estimate_state.X);
}

fn sqr<T: Copy + Mul<Output = T>>(x: T) -> T {
    x * x
}