// This is a minimal example of using the LQR controller of the library to compute the optimal
// feedback controls for a car with the kinematic bicycle model
use lqr::LQRController;
use nalgebra as na;

fn main() -> Result<(), &'static str> {
    // Define state
    let x: f64 = 2.0;
    let y: f64 = 2.0;
    let theta: f64 = 0.34;
    let v: f64 = 3.0;

    // Define controls
    let delta: f64 = 0.0;
    let acc: f64 = 0.0;

    // Model parameters
    let l = 2.0; // wheelbase

    // compute matrices for the LQR controller
    let a = na::Matrix4::<f64>::new(
        0.0,
        0.0,
        -v * theta.sin(),
        theta.cos(),
        0.0,
        0.0,
        v * theta.cos(),
        theta.sin(),
        0.0,
        0.0,
        0.0,
        delta.tan() / l,
        0.0,
        0.0,
        0.0,
        0.0,
    );
    let b = na::Matrix4x2::<f64>::new(
        0.0,
        0.0,
        0.0,
        0.0,
        v / (l * delta.cos().powf(2.0)),
        0.0,
        0.0,
        1.0,
    );
    let q = na::Matrix4::identity();
    let r = na::Matrix2::identity();

    let mut controller = LQRController::new()?;
    controller.compute_gain(&a, &b, &q, &r, 1e-6)?;

    println!("Found gain matrix {}", controller.k.unwrap());

    // Set states for the optimal control computation
    let current_state = na::Vector4::<f64>::new(x, y, theta, v);
    let desired_state = na::Vector4::<f64>::new(x + 1.0, y, theta + 0.01, 3.0);

    let u_feedforward = na::Vector2::<f64>::new(acc, delta);
    let u_feedback = controller.compute_optimal_controls(&current_state, &desired_state)?;
    let u = u_feedforward + u_feedback;

    println!("From state {}", current_state);
    println!("To state {}", desired_state);
    println!("Computed optimal controls {}", u);

    return Ok(());
}