Implementing waypoints and also route optimization