#include "odeexprk4.h"

void OdeExpRk41::step(Vecd &xout, const OdeFunc1 &f, const Vecd &x, double h) const
{
    Vecd k1(h*f(x));
    Vecd k2(h*f(x+k1/2));
    Vecd k3(h*f(x+k2/2));
    Vecd k4(h*f(x+k3));

    xout = x;
    xout += k1/6;
    xout += k2/3;
    xout += k3/3;
    xout += k4/6;
}

void OdeExpRk42::step(Vecd &xout, Vecd &vout, const OdeFunc2 &f, const Vecd &x, const Vecd &v, double h) const
{
    Vecd k1x = h*v;
    Vecd k1v = h*f(x,v);

    Vecd k2x = h*(v+k1v/2);
    Vecd k2v = h*f(x+k1x/2,v+k1v/2);

    Vecd k3x = h*(v+k2v/2);
    Vecd k3v = h*f(x+k2x/2,v+k2v/2);

    Vecd k4x = h*(v+k3v);
    Vecd k4v = h*f(x+k3x,x+k3v);

//    cout << k1x << k2x << k3x << k4x << endl;

    xout = x;
    xout += k1x/6;
    xout += k2x/3;
    xout += k3x/3;
    xout += k4x/6;

    vout = v;
    vout += k1v/6;
    vout += k2v/3;
    vout += k3v/3;
    vout += k4v/6;
}
