Modify drive function to work in temporal domain.
Add function get_current_waypoint to set the waypoint of the
reference path based on the traveled distance s of the car.
Add comments. SimpleBicycleModel checked.
New Convention:
each function call with control signals
(D, delta) according to order in control signal returned by MPC