Added code for work out route, work out route points
Merge remote-tracking branch 'origin/master'