WIP - the smooth planner code to compute and assign times and velocities to a path

This does not compile yet.  I just want to check point my work thus far