Adaptive stepsize seems to be working

src/lib.rs

@@ -1,14 +1,48 @@
-pub fn add(left: usize, right: usize) -> usize {
-    left + right
-}
+#![allow(dead_code)]
+
+pub mod ode;
+pub mod integrator;
+pub mod controller;
+// pub mod callback;
+pub mod problem;
+
 
 #[cfg(test)]
 mod tests {
-    use super::*;
+    use nalgebra::Vector6;
+    use approx::assert_relative_eq;
+    use crate::integrator::{AdaptiveSixthOrder, RUNGE_KUTTA_FEHLBERG_54_TABLEAU};
+    use crate::controller::{PIController};
+    use crate::ode::{SystemTrait, ODE};
+    use crate::problem::Problem;
 
     #[test]
-    fn it_works() {
-        let result = add(2, 2);
-        assert_eq!(result, 4);
+    fn test_ode_creation() {
+        struct System {
+            mu: f64,
+        }
+
+        impl SystemTrait<f64,6> for System {
+            fn derivative(&self, _t: f64, state: Vector6<f64>) -> Vector6<f64> {
+                let acc = -(self.mu * 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 system = System { mu: 3.98600441500000e14 };
+        let y0 = Vector3::new(1.0, 0.0, 0.0);
+        let ode = ODE::new(system, 0.0, 10.0, y0);
+        let controller = PIController::new(0.17, 0.04, 10.0, 0.2, 10.0, 0.9, 1e-4);
+        let rkf54 = AdaptiveSixthOrder {
+            tableau: RUNGE_KUTTA_FEHLBERG_54_TABLEAU,
+            atol: 1e-8_f64,
+            rtol: 1e-8_f64,
+        };
+        let mut problem = Problem::new(ode, rkf54, controller);
+
+        let solution = problem.solve();
+        solution.times.iter().zip(solution.states.iter()).for_each(|(time, state)| {
+            assert_relative_eq!(state[0], time.exp(), max_relative=1e-7);
+        })
     }
 }