First pass fix smooth path velocities accounting for curvature

Added code to handle boundary conditions when fitting velocities
to the beginning and end of the path.  Changed how time stamps,
velocities, and acceleration are computed.  This code barely compiles
and is guaranteed to contain swarms of bugs