Added code to advance current position along route (untested)