use nalgebra::SVector;

use super::ode::ODE;

pub mod dormand_prince;
// pub mod rosenbrock;

/// Integrator Trait
pub trait Integrator<const D: usize> {
    const ORDER: usize;
    const STAGES: usize;
    const ADAPTIVE: bool;
    const DENSE: bool;
    /// Returns a new y value, then possibly an error value, and possibly a dense output
    /// coefficient set
    fn step<P>(
        &self,
        ode: &ODE<D, P>,
        h: f64,
    ) -> (SVector<f64, D>, Option<f64>, Option<Vec<SVector<f64, D>>>);
    fn interpolate(
        &self,
        t_start: f64,
        t_end: f64,
        dense: &[SVector<f64, D>],
        t: f64,
    ) -> SVector<f64, D>;
}

#[cfg(test)]
mod tests {
    use super::dormand_prince::*;
    use super::*;
    use approx::assert_relative_eq;
    use nalgebra::Vector3;

    #[test]
    fn test_dopri() {
        type Params = ();
        fn derivative(_t: f64, y: Vector3<f64>, _p: &Params) -> Vector3<f64> {
            y
        }

        let y0 = Vector3::new(1.0, 1.0, 1.0);
        let mut ode = ODE::new(&derivative, 0.0, 4.0, y0, ());

        let dp45 = DormandPrince45::new().a_tol(1e-12).r_tol(1e-4);

        // Test that y'(t) = y(t) solves to y(t) = e^t for rkf54
        // and also that the error seems reasonable
        let step = 0.001;
        while ode.t < ode.t_end {
            let (new_y, err, _) = dp45.step(&ode, step);
            ode.y = new_y;
            ode.t += step;
            assert_relative_eq!(ode.y[0], ode.t.exp(), max_relative = 0.01);
            assert!(err.unwrap() < 1.0);
        }
    }
}