Start rewrite to use RK-4 constant step size and autodiff
Start writing OCProblem class
adding on to OCProblem