Reimplemented RK4