Pull mult_shoot into it's own class, try to generalize, at least for casadi