RK4 code and diagnostics added