#![allow(dead_code)]

pub mod ode;
pub mod integrator;
pub mod controller;
pub mod callback;
pub mod problem;

pub mod prelude {
    pub use super::ode::ODE;
    pub use super::integrator::dormand_prince::DormandPrince45;
    pub use super::controller::PIController;
    pub use super::callback::{Callback, stop};
    pub use super::problem::{Problem, Solution};
}

#[cfg(test)]
mod tests {
    use nalgebra::{Vector3, Vector6};
    use approx::assert_relative_eq;
    use crate::prelude::*;
    use std::f64::consts::PI;

    #[test]
    fn test_readme() {
        // Define the system (parameters, derivative, and initial state)
        type Params = (f64, bool);
        let params = (34.0, true);

        fn derivative(t: f64, y: Vector3<f64>, p: &Params) -> Vector3<f64> {
          if p.1 { -y } else { y * t }
        }

        let y0 = Vector3::new(1.0, 1.0, 1.0);

        // Set up the problem (ODE, Integrator, Controller, and Callbacks)
        let ode = ODE::new(&derivative, 0.0, 10.0, y0, params);
        let dp45 = DormandPrince45::new(1e-12_f64, 1e-5_f64);
        let controller = PIController::default();

        let value_too_high = Callback {
            event: &|_: f64, y: Vector3<f64>, _: &Params| { 10.0 - y[0] },
            effect: &stop,
        };

        // Solve the problem
        let mut problem = Problem::new(ode, dp45, controller).with_callback(value_too_high);
        let solution = problem.solve();

        // Can interpolate solutions to whatever you want
        let _interpolated_answer = solution.interpolate(8.2);
    }

    #[test]
    fn test_orbit() {
        // Calculate one period
        let a = 6.7781363e6_f64;
        let mu = 3.98600441500000e14;
        let period = 2.0 * PI * (a.powi(3)/mu).sqrt();

        // Set up the system
        type Params = (f64,);
        let params = (mu,);
        fn derivative(_t: f64, state: Vector6<f64>, p: &Params) -> Vector6<f64> {
            let acc = -(p.0 * state.fixed_rows::<3>(0)) / (state.fixed_rows::<3>(0).norm().powi(3));
            Vector6::new(state[3], state[4], state[5], acc[0], acc[1], acc[2])
        }
        let y0 = Vector6::new(
            4.263868426884883e6,
            5.146189057155391e6,
            1.1310208421331816e6,
            -5923.454461876975,
            4496.802639690076,
            1870.3893008991558,
        );

        // Integrate
        let ode = ODE::new(&derivative, 0.0, 10.0*period, y0, params);
        let dp45 = DormandPrince45::new(1e-12_f64, 1e-12_f64);
        let controller = PIController::new(0.37, 0.04, 10.0, 0.2, 1000.0, 0.9, 0.01);

        let mut problem = Problem::new(ode, dp45, controller);

        let solution = problem.solve();

        assert_relative_eq!(solution.times[solution.states.len()-1], 10.0 * period, max_relative=1e-12);
        assert_relative_eq!(solution.states[solution.states.len()-1][0], y0[0], max_relative=1e-9);
        assert_relative_eq!(solution.states[solution.states.len()-1][1], y0[1], max_relative=1e-9);
        assert_relative_eq!(solution.states[solution.states.len()-1][2], y0[2], max_relative=1e-9);
        assert_relative_eq!(solution.states[solution.states.len()-1][3], y0[3], max_relative=1e-9);
        assert_relative_eq!(solution.states[solution.states.len()-1][4], y0[4], max_relative=1e-9);
        assert_relative_eq!(solution.states[solution.states.len()-1][5], y0[5], max_relative=1e-9);
    }
}