use lyapunov::states::ThetaOmegaState;
use lyapunov::traits::Saturate;
use lyapunov::units::*;
use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4};

fn main() {
    let system = Pendulum::new(
        Mass::new::<kilogram>(0.024),
        Length::new::<meter>(0.129),
        MomentOfInertia::new::<kilogram_square_meter>(3.33 * 0.00001),
        Acceleration::new::<meter_per_second_squared>(9.81),
        DCMotor::qube_servo_2(),
    );

    let r = ThetaOmegaState::new(Angle::new::<radian>(std::f64::consts::PI));

    let feedback = LinearFeedback::<2, 1>::pendulum(&system);

    let rk4 = Rk4;

    // Start at the downward (stable) equilibrium to demonstrate swing-up.
    let mut x = ThetaOmegaState::zero();

    let dt = Time::new::<millisecond>(10.0);

    for step in 0..350 {
        let t = dt * (step as f64);

        let u_fb = feedback.calculate(&system, t, &x, &r);
        let u_ff = system.gravity_cancelling_input(&x);
        let u_total = u_fb + u_ff;
        let u = u_total.saturate(&system);

        x = rk4.step(&system, t, &x, &u, dt);

        println!(
            "t={:.2?}  theta={:.3?}  omega={:.3?}  current={:.3?}  u_fb={:.3?}  u_ff={:.3?}",
            t,
            x.angle(),
            x.angular_velocity(),
            u.current(),
            u_fb.current(),
            u_ff.current()
        );
    }
}