Off path fixes

Logic not quite right when checking if we had been heading off path.
Off path fixes 2

Fix bearing difference, index path points, refactor NavigationState