pick instruction and additionally consider the heading

src/stores/TurnNavigationStore.ts

@@ -233,7 +233,7 @@ export default class TurnNavigationStore extends Store<TurnNavigationStoreState>
 
             const coordinate = action.coordinate
             let path = state.activePath
-            let instrInfo = getCurrentInstruction(path.instructions, coordinate)
+            let instrInfo = getCurrentInstruction(path.instructions, coordinate, action.heading)
 
             // skip waypoint if close to it and next is available (either activePath has via points or initialPath)
             let skipWaypoint = TurnNavigationStore.skipWaypoint(
@@ -250,7 +250,7 @@ export default class TurnNavigationStore extends Store<TurnNavigationStoreState>
             ) {
                 // switch back to original path and skip the current waypoint
                 path = state.initialPath
-                instrInfo = getCurrentInstruction(path.instructions, coordinate)
+                instrInfo = getCurrentInstruction(path.instructions, coordinate, action.heading)
                 skipWaypoint = TurnNavigationStore.skipWaypoint(
                     state.instruction.distanceToWaypoint,
                     TurnNavigationStore.getWaypoint(path, instrInfo.nextWaypointIndex),
@@ -401,7 +401,7 @@ export default class TurnNavigationStore extends Store<TurnNavigationStoreState>
             const path = action.path
 
             // ensure that path and instruction are synced
-            const instr = getCurrentInstruction(path.instructions, state.coordinate)
+            const instr = getCurrentInstruction(path.instructions, state.coordinate, state.heading)
 
             // current location is still not close
             if (instr.index < 0) {

src/turnNavigation/GeoMethods.ts

@@ -31,7 +31,8 @@ export function getCurrentDetails(path: Path, pillarPoint: Coordinate, details:
 
 export function getCurrentInstruction(
     instructions: Instruction[],
-    location: Coordinate
+    location: Coordinate,
+    heading: undefined | number
 ): {
     index: number
     timeToTurn: number
@@ -43,13 +44,9 @@ export function getCurrentInstruction(
     distanceToWaypoint: number
     nextWaypointIndex: number
 } {
-    let instructionIndex = -1
-    let distanceToRoute = Number.MAX_VALUE
-    // TODO do we need to calculate the more precise route distance or is the current straight-line distance sufficient?
-    let distanceToTurn = -1
-    let nextWaypointIndex = 0
     let waypointIndex = 0
-    let pillarPointOnRoute = { lat: 0, lng: 0 }
+    const result = new InstructionResult()
+    const resultWithHeadingFilter = new InstructionResult()
 
     for (let instrIdx = 0; instrIdx < instructions.length; instrIdx++) {
         const sign = instructions[instrIdx].sign
@@ -59,41 +56,56 @@ export function getCurrentInstruction(
         for (let pIdx = 0; pIdx < points.length; pIdx++) {
             const p: number[] = points[pIdx]
             let snapped = { lat: p[1], lng: p[0] }
+            const last: number[] = points[points.length - 1]
             let dist = calcDist(snapped, location)
-            // calculate the snapped point, TODO use first point of next instruction for "next" if last point of current instruction
+            let headingMatches = false
+            // calculate the snapped point
+            // TODO use first point of next instruction for "next" if last point of current instruction
             if (pIdx + 1 < points.length) {
                 const next: number[] = points[pIdx + 1]
                 if (validEdgeDistance(location.lat, location.lng, p[1], p[0], next[1], next[0])) {
                     snapped = calcCrossingPointToEdge(location.lat, location.lng, p[1], p[0], next[1], next[0])
                     dist = Math.min(dist, calcDist(snapped, location))
                 }
+
+                if (heading) {
+                    // TODO reject point based on heading if a similar close point is available
+                    const tmpHeading = toDegrees(toNorthBased(calcOrientation(p[1], p[0], next[1], next[0])))
+                    headingMatches = Math.abs(heading - tmpHeading) < 40
+                }
             }
 
-            if (dist < distanceToRoute) {
-                distanceToRoute = dist
+            const set = (res: InstructionResult) => {
+                res.distanceToRoute = dist
                 // use next instruction or finish
-                instructionIndex = instrIdx + 1 < instructions.length ? instrIdx + 1 : instrIdx
-                const last: number[] = points[points.length - 1]
-                distanceToTurn = Math.round(calcDist({ lat: last[1], lng: last[0] }, snapped))
-                nextWaypointIndex = waypointIndex + 1
-                pillarPointOnRoute = { lat: p[1], lng: p[0] }
+                res.index = instrIdx + 1 < instructions.length ? instrIdx + 1 : instrIdx
+                res.distanceToTurn = Math.round(calcDist({ lat: last[1], lng: last[0] }, snapped))
+                res.nextWaypointIndex = waypointIndex + 1
+                res.pillarPointOnRoute = { lat: p[1], lng: p[0] }
             }
+
+            if (dist < result.distanceToRoute) set(result)
+            if (dist < resultWithHeadingFilter.distanceToRoute && headingMatches) set(resultWithHeadingFilter)
         }
     }
 
+    const finalResult =
+        resultWithHeadingFilter.index >= 0 && resultWithHeadingFilter.distanceToRoute < 20
+            ? resultWithHeadingFilter
+            : result
     let distanceToWaypoint = -1
     let timeToTurn = 0
     let timeToEnd = 0
-    let distanceToEnd = distanceToTurn
-    if (instructionIndex >= 0) {
-        if (instructionIndex > 0) {
+    let distanceToEnd = finalResult.distanceToTurn
+    if (finalResult.index >= 0) {
+        if (finalResult.index > 0) {
             // proportional estimate the time to the next instruction, TODO use time from path details instead
-            let prevInstr = instructions[instructionIndex - 1]
+            let prevInstr = instructions[finalResult.index - 1]
             timeToTurn = prevInstr.distance > 0 ? prevInstr.time * (distanceToEnd / prevInstr.distance) : 0
         }
         timeToEnd = timeToTurn
-        distanceToEnd = distanceToTurn
-        for (let instrIdx = instructionIndex; instrIdx < instructions.length; instrIdx++) {
+        distanceToEnd = finalResult.distanceToTurn
+        for (let instrIdx = finalResult.index; instrIdx < instructions.length; instrIdx++) {
             timeToEnd += instructions[instrIdx].time
             distanceToEnd += instructions[instrIdx].distance
 
@@ -105,18 +117,23 @@ export function getCurrentInstruction(
     }
 
     return {
-        index: instructionIndex,
+        ...finalResult,
         timeToTurn,
-        distanceToTurn,
-        distanceToRoute,
-        pillarPointOnRoute,
         timeToEnd,
         distanceToEnd,
         distanceToWaypoint,
-        nextWaypointIndex,
     }
 }
 
+class InstructionResult {
+    index = -1
+    distanceToRoute = Number.MAX_VALUE
+    // TODO do we need to calculate the more precise route distance or is the current straight-line distance sufficient?
+    distanceToTurn = -1
+    nextWaypointIndex = 0
+    pillarPointOnRoute = { lat: 0, lng: 0 }
+}
+
 /**
  * Calculates the great-circle distance between two points on Earth given the latitudes and longitudes
  * assuming that Earth is a sphere with radius 6371km. The result is returned in meters.

test/stores/TurnNavigationStore.test.ts

@@ -25,6 +25,8 @@ let reroute2 = toRoutingResult(require('../turnNavigation/reroute2.json'))
 let announceBug = toRoutingResult(require('../turnNavigation/announce-bug-original.json'))
 let announceBugReroute = toRoutingResult(require('../turnNavigation/announce-bug-reroute.json'))
 
+let loopBug = toRoutingResult(require('../turnNavigation/loop-bug.json'))
+
 function toRoutingResult(rawResult: RawResult): RoutingResult {
     return {
         ...rawResult,
@@ -311,6 +313,49 @@ describe('TurnNavigationStore', () => {
 
             expect(speech.getTexts()).toEqual(['Links halten', 'reroute', 'Scharf links abbiegen'])
         })
+
+        it('do not announce too old instruction for loops', async () => {
+            const api = new LocalApi()
+            const speech = new DummySpeech()
+            const store = createStore(api, speech)
+            Dispatcher.dispatch(new SetVehicleProfile({ name: 'car' }))
+            Dispatcher.dispatch(new SetSelectedPath(loopBug.paths[0]))
+            Dispatcher.dispatch(new TurnNavigationSettingsUpdate({ soundEnabled: true } as TNSettingsState))
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.97108, lat: 50.352875 }, true, 16, 135))
+            expect(store.state.activePath).toEqual(loopBug.paths[0])
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.972844, lat: 50.350855 }, true, 16, 180))
+
+            // GPS location is closer to incorrect (underlying) motorway than to bridge.
+            // Due to heading prefer the slightly more distant bridge
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.972071, lat: 50.351871 }, true, 16, 45))
+
+            expect(speech.getTexts()).toEqual([
+                'Keep right and take B 173 toward Hof-Zentrum, Feilitzsch, Trogen',
+                'Turn right onto B 173',
+                'Arrive at destination',
+            ])
+        })
+
+        it('do not announce future instruction for loops', async () => {
+            const api = new LocalApi()
+            const speech = new DummySpeech()
+            const store = createStore(api, speech)
+            Dispatcher.dispatch(new SetVehicleProfile({ name: 'car' }))
+            Dispatcher.dispatch(new SetSelectedPath(loopBug.paths[0]))
+            Dispatcher.dispatch(new TurnNavigationSettingsUpdate({ soundEnabled: true } as TNSettingsState))
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.97108, lat: 50.352875 }, true, 16, 135))
+            expect(store.state.activePath).toEqual(loopBug.paths[0])
+
+            // GPS location is closer to bridge than to correct motorway -> with heading still enforces motorway
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.97213, lat: 50.351902 }, true, 16, 135))
+            // trigger announcements to turn right
+            Dispatcher.dispatch(new LocationUpdate({ lng: 11.972865, lat: 50.350855 }, true, 16, 180))
+
+            expect(speech.getTexts()).toEqual([
+                'Keep right and take B 173 toward Hof-Zentrum, Feilitzsch, Trogen',
+                'Turn right onto B 173',
+            ])
+        })
     })
 
     function createStore(api: Api, speech = new DummySpeech()) {

test/turnNavigation/GeoMethods.test.ts

@@ -11,10 +11,14 @@ describe('calculate instruction', () => {
     // http://localhost:3000/?point=51.437233%2C14.246489&point=51.435514%2C14.239923&profile=car
     it('second instruction should not be "right turn"', () => {
         let path = ApiImpl.decodeResult(responseHoyerswerda1, true)[0]
-        const { index, distanceToTurn, timeToEnd, distanceToEnd } = getCurrentInstruction(path.instructions, {
-            lat: 51.435029,
-            lng: 14.243259,
-        })
+        const { index, distanceToTurn, timeToEnd, distanceToEnd } = getCurrentInstruction(
+            path.instructions,
+            {
+                lat: 51.435029,
+                lng: 14.243259,
+            },
+            undefined
+        )
 
         expect(distanceToTurn).toEqual(236)
         expect(distanceToEnd).toEqual(236)
@@ -25,10 +29,14 @@ describe('calculate instruction', () => {
 
     it('remaining time should be correct', () => {
         let path = ApiImpl.decodeResult(responseHoyerswerda1, true)[0]
-        const { timeToEnd, distanceToEnd } = getCurrentInstruction(path.instructions, {
-            lat: 51.439291,
-            lng: 14.245254,
-        })
+        const { timeToEnd, distanceToEnd } = getCurrentInstruction(
+            path.instructions,
+            {
+                lat: 51.439291,
+                lng: 14.245254,
+            },
+            undefined
+        )
 
         expect(Math.round(timeToEnd / 1000)).toEqual(101)
         expect(Math.round(distanceToEnd)).toEqual(578)
@@ -37,32 +45,61 @@ describe('calculate instruction', () => {
     it('nextWaypointIndex should be correct', () => {
         let path = ApiImpl.decodeResult(responseHoyerswerda2, true)[0]
         {
-            const { nextWaypointIndex } = getCurrentInstruction(path.instructions, {
-                lat: 51.434672,
-                lng: 14.267248,
-            })
+            const { nextWaypointIndex } = getCurrentInstruction(
+                path.instructions,
+                {
+                    lat: 51.434672,
+                    lng: 14.267248,
+                },
+                undefined
+            )
             expect(nextWaypointIndex).toEqual(1)
         }
 
         // points that could return both indices return the first
         // TODO include heading to differentiate!
         {
-            const { nextWaypointIndex } = getCurrentInstruction(path.instructions, {
-                lat: 51.434491,
-                lng: 14.268535,
-            })
+            const { nextWaypointIndex } = getCurrentInstruction(
+                path.instructions,
+                {
+                    lat: 51.434491,
+                    lng: 14.268535,
+                },
+                undefined
+            )
             expect(nextWaypointIndex).toEqual(1)
         }
 
         {
-            const { nextWaypointIndex } = getCurrentInstruction(path.instructions, {
-                lat: 51.433247,
-                lng: 14.267763,
-            })
+            const { nextWaypointIndex } = getCurrentInstruction(
+                path.instructions,
+                {
+                    lat: 51.433247,
+                    lng: 14.267763,
+                },
+                undefined
+            )
             expect(nextWaypointIndex).toEqual(2)
         }
     })
 
+    it('pick instruction depending on heading for same location', () => {
+        let path = ApiImpl.decodeResult(responseHoyerswerda2, true)[0]
+        const location = { lat: 51.434356, lng: 14.267697 }
+        {
+            const { index } = getCurrentInstruction(path.instructions, location, 170)
+            expect(path.instructions[index].text).toEqual('Turn left onto Franz-Liszt-Straße')
+        }
+        {
+            const { index } = getCurrentInstruction(path.instructions, location, 240)
+            expect(path.instructions[index].text).toEqual('Turn left onto Bautzener Allee')
+        }
+        {
+            const { index } = getCurrentInstruction(path.instructions, location, 80)
+            expect(path.instructions[index].text).toEqual('Waypoint 1')
+        }
+    })
+
     it('calc angle', () => {
         // downwards+west
         expect(Math.round(toDegrees(calcOrientation(51.439146, 14.245258, 51.438908, 14.245931)))).toEqual(-30)