use crate::{algebra::Scalar, particle_system::ParticleSystem};
use super::{PhaseSpace, Solver};

// Runge-Kutta 4th order
impl Solver for ParticleSystem {
    fn step(&mut self, dt: Scalar) {
        let state = self.collect_phase_space();

        // k1 = f(t, y)
        let k1 = self.compute_derivative();

        // k2 = f(t + dt/2, y + dt * k1 / 2)
        self.scatter_phase_space(&PhaseSpace {
            0: state.0.clone() + k1.0.clone() * dt / 2.0,
        });
        let k2 = self.compute_derivative();


        // k3 = f(t + dt/2, y + dt * k2 / 2)
        self.scatter_phase_space(&PhaseSpace {
            0: state.0.clone() + k2.0.clone() * dt / 2.0,
        });
        let k3 = self.compute_derivative();

        // k4 = f(t + dt, y + dt * k3)
        self.scatter_phase_space(&PhaseSpace {
            0: state.0.clone() + k3.0.clone() * dt
        });
        let k4 = self.compute_derivative();

        self.scatter_phase_space(&PhaseSpace {
            0: state.0.clone() + (k1.0 + 2.0 * k2.0 + 2.0 * k3.0 + k4.0) * dt / 6.0
        });

        self.t += dt;
    }
}