RouteCalculator

app/build.gradle.kts

@@ -17,8 +17,8 @@ android {
         applicationId = "com.kouros.navigation"
         minSdk = 33
         targetSdk = 37
-        versionCode = 108
+        versionCode = 109
-        versionName = "0.3.0.108"
+        versionName = "0.3.0.109"
         base.archivesName = "navi-$versionName"
         testInstrumentationRunner = "androidx.test.runner.AndroidJUnitRunner"
     }

common/car/src/main/java/com/kouros/navigation/car/DeviceLocationManager.kt

@@ -107,8 +107,8 @@ class DeviceLocationManager(
         val lastLocation = locationManager.getLastKnownLocation(LocationManager.GPS_PROVIDER)
         if (lastLocation != null) {
             if (setIndividualLocation) {
-                onInitialLocation(lastLocation)
+                onInitialLocation(homeVogelhart)
-                onLocationUpdate(lastLocation)
+                onLocationUpdate(homeVogelhart)
             } else {
                 onInitialLocation(lastLocation)
                 onLocationUpdate(lastLocation)

common/car/src/main/java/com/kouros/navigation/car/NavigationSession.kt

@@ -485,6 +485,7 @@ class NavigationSession : CarSession(), NavigationListener, NavigationObserverCa
         val distance = location.distanceTo(snappedLocation)
         when {
             distance > MAXIMAL_ROUTE_DEVIATION -> {
+                Log.d(TAG, "Maximal route deviation")
                 stopNavigation()
                 navigationScreen.calculateNewRoute(routeModel.navState.destination)
                 return false

common/car/src/main/java/com/kouros/navigation/car/navigation/Simulation.kt

@@ -28,8 +28,8 @@ class Simulation {
     ) {
         if (routeModel.navState.route.isRouteValid()) {
             if (BuildConfig.DEBUG) {
-                gpxSimulation(routeModel, lifecycleScope, updateLocation)
+                //gpxSimulation(routeModel, lifecycleScope, updateLocation)
-                //currentSimulation(routeModel, lifecycleScope, updateLocation)
+                currentSimulation(routeModel, lifecycleScope, updateLocation)
             } else {
                 currentSimulation(routeModel, lifecycleScope, updateLocation)
             }
@@ -46,7 +46,7 @@ class Simulation {
         if (points.isEmpty()) return
         simulationJob?.cancel()
         var lastLocation = Location(LocationManager.FUSED_PROVIDER)
-        var curBearing = 0f
+        var curBearing: Float
         simulationJob = lifecycleScope.launch {
             for ((index, point) in points.withIndex()) {
                 if (index >= 0) {
@@ -90,7 +90,7 @@ class Simulation {
         simulationJob?.cancel()
         simulationJob =lifecycleScope.launch() {
             var lastLocation = Location(LocationManager.FUSED_PROVIDER)
-            var curBearing = 0f
+            val curBearing = 0f
             val parser = GPXParser()
             val parsedGpx: Gpx? =
                 parser.parse(route.byteInputStream())
@@ -118,9 +118,6 @@ class Simulation {
                             // Update your app's state as if a real GPS update occurred
                             updateLocation(fakeLocation)
                             // Wait before moving to the next point (e.g., every 1 second)
-                            if (duration > 100) {
-                             //   delay(duration / 4)
-                            }
                             delay(200)
                             lastTime = p.time
                             lastLocation = fakeLocation

common/data/src/main/java/com/kouros/navigation/data/Data.kt

@@ -16,6 +16,7 @@
 
 package com.kouros.navigation.data
 
+import android.location.Location
 import android.net.Uri
 import com.google.gson.annotations.Expose
 import com.kouros.navigation.data.route.Lane
@@ -28,7 +29,7 @@ data class Category(
     val name: String,
 )
 
-
+data class StepMatch(val stepIndex: Int, val waypointIndex: Int, val location: Location)
 
 data class Places(
     val places: List<Place>,
@@ -144,7 +145,7 @@ object Constants {
 
     const val NEAREST_LOCATION_DISTANCE = 10F
 
-    const val MAXIMUM_LOCATION_DISTANCE = 100000F
+    const val MAXIMUM_LOCATION_DISTANCE = Float.MAX_VALUE
 
     const val TRAFFIC_UPDATE = 300
 

common/data/src/main/java/com/kouros/navigation/model/RouteCalculator.kt

@@ -3,60 +3,84 @@ package com.kouros.navigation.model
 import android.location.Location
 import android.util.Log
 import androidx.car.app.navigation.model.Step
-import com.kouros.navigation.data.ApplicationConfig
 import com.kouros.navigation.data.Constants.MAXIMUM_LOCATION_DISTANCE
 import com.kouros.navigation.data.Constants.NEAREST_LOCATION_DISTANCE
 import com.kouros.navigation.data.Constants.SPEED_UPDATE_DISTANCE
 import com.kouros.navigation.data.Constants.TAG
-import com.kouros.navigation.utils.bearingPositive
+import com.kouros.navigation.data.StepMatch
+import com.kouros.navigation.data.osrm.Waypoints
 import com.kouros.navigation.utils.location
 import java.util.concurrent.TimeUnit
 import kotlin.math.absoluteValue
 
 class RouteCalculator(var routeModel: RouteModel) {
 
+    var bestMatch: StepMatch? = null
     var lastSpeedLocation: Location = location(0.0, 0.0)
 
     var lastSpeedIndex: Int = 0
 
     fun findStep(location: Location) {
+        val route = routeModel.navState.route
+        val steps = routeModel.curLeg.steps
+        val startIndex = route.currentStepIndex
+
+        // Windowed search for performance (current + 2 steps)
+        //val endIndex = (startIndex + 2).coerceAtMost(steps.size - 1)
+        val endIndex = steps.size - 1
+
         var nearestDistance = MAXIMUM_LOCATION_DISTANCE
-        var count = 0
+        var searchCount = 0
-        var lastDistance = 0F
+
-        var increaseDistance = 0
+        for (i in startIndex..endIndex) {
-        for ((index, step) in routeModel.curLeg.steps.withIndex()) {
+            val step = steps[i]
-            count++
+            val waypoints = step.maneuver.waypoints
-            var distance = 0F
+            val startWayIndex = if (i == startIndex) step.waypointIndex else 0
-            if (index >= routeModel.navState.route.currentStepIndex) {
+
-                for ((wayIndex, waypoint) in step.maneuver.waypoints.withIndex()) {
+            var lastDistance = Float.MAX_VALUE
-                    count++
+            var increaseCount = 0
-                    if (wayIndex >= step.waypointIndex) {
+
-                        distance = location.distanceTo(waypoint)
+            for (j in startWayIndex until waypoints.size) {
-                        if (distance < nearestDistance  ) {
+                searchCount++
-                            nearestDistance = distance
+                val waypoint = waypoints[j]
-                            routeModel.navState.route.currentStepIndex = step.index
+                val distance = location.distanceTo(waypoint)
-                            step.waypointIndex = wayIndex
+
-                            step.wayPointLocation = waypoint
+                if (distance < nearestDistance) {
-                        }
+                    nearestDistance = distance
-                    }
+                    bestMatch = StepMatch(step.index, j, waypoint)
-                    if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
-                        break
-                    }
-                    if (distance > lastDistance) {
-                        increaseDistance++
-                    }
-                    lastDistance = distance
                 }
-            }
+
-            if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
+                // Track if we are getting further away
-                break
+                if (distance > lastDistance) {
+                    increaseCount++
+                } else {
+                    increaseCount = 0
+                }
+                // Absolute early exit: found a close point and now moved far away
+                if (stopSearch(nearestDistance, distance, increaseCount)) {
+                    bestMatch?.let { match ->
+                        route.currentStepIndex = match.stepIndex
+                        steps[match.stepIndex].waypointIndex = match.waypointIndex
+                        steps[match.stepIndex].wayPointLocation = match.location
+                    }
+                    Log.d(TAG, "FindStep optimized: count=$searchCount, dist=$nearestDistance")
+                    return
+                }
+                lastDistance = distance
             }
         }
-        Log.d(TAG, "FindStep $nearestDistance $count $increaseDistance")
+        bestMatch?.let { match ->
+            route.currentStepIndex = match.stepIndex
+            steps[match.stepIndex].waypointIndex = match.waypointIndex
+            steps[match.stepIndex].wayPointLocation = match.location
+
+        }
+        Log.d(TAG, "FindStep not optimized: count=$searchCount, dist=$nearestDistance")
     }
 
-    private fun stopSearch(nearestDistance: Float, distance: Float): Boolean {
+    private fun stopSearch(nearestDistance: Float, distance: Float, increaseCount: Int): Boolean {
-        return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10
+        return (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10)
+                || increaseCount > 10
     }
 
     fun travelLeftTime(): Double {
@@ -92,7 +116,7 @@ class RouteCalculator(var routeModel: RouteModel) {
     fun leftStepDistance(): Double {
         val step = routeModel.route.currentStep()
         var leftDistance = 0F
-        if (step.waypointIndex < step.maneuver.waypoints.size -1) {
+        if (step.waypointIndex < step.maneuver.waypoints.size - 1) {
             leftDistance = step.maneuver.leftDistance[step.waypointIndex]
             val waypointLocation = step.maneuver.waypoints[step.waypointIndex]
             if (routeModel.navState.lastLocation.latitude != 0.0) {

common/data/src/test/java/com/kouros/navigation/model/RouteCalculatorTest.kt

@@ -7,6 +7,7 @@ import com.kouros.navigation.data.route.Maneuver
 import com.kouros.navigation.data.route.Routes
 import com.kouros.navigation.data.route.Step
 import com.kouros.navigation.data.route.Summary
+import com.kouros.navigation.utils.location
 import org.junit.Assert.assertEquals
 import org.junit.Assert.assertTrue
 import org.junit.Before
@@ -44,7 +45,7 @@ class RouteCalculatorTest {
         waypointIndex: Int = 0,
     ): Step {
         val waypoints = (0 until numWaypoints).map { i ->
-            listOf(11.0 + index * 0.01 + i * 0.001, 48.0)
+            location(11.0 + index * 0.01 + i * 0.001, 48.0)
         }
         return Step(
             index = index,
@@ -127,13 +128,14 @@ class RouteCalculatorTest {
         )
 
         val mockLocation: Location = mock()
-        // step0/wp0: 500F, step0/wp1: 5F — 5F < NEAREST_LOCATION_DISTANCE (10F) → break
+        // step0/wp0: 500F, step0/wp1: 5F, step1/wp0: 200F
-        whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F)
+        // 5F < NEAREST_LOCATION_DISTANCE (10F) AND 200F > 10 * 10F → break
+        whenever(mockLocation.distanceTo(any())).thenReturn(500F, 5F, 200F)
 
         routeCalculator.findStep(mockLocation)
 
-        // step1 and step2 are never evaluated
+        // should stop after the 3rd call (step1/wp0 triggers stopSearch)
-        verify(mockLocation, times(2)).distanceTo(any())
+        verify(mockLocation, times(3)).distanceTo(any())
         assertEquals(0, routeModel.navState.route.currentStepIndex)
     }
 

common/data/src/test/java/com/kouros/navigation/model/RouteModelTest.kt

@@ -9,6 +9,7 @@ import com.kouros.navigation.data.route.Routes
 import com.kouros.navigation.data.route.Step
 import com.kouros.navigation.data.route.Summary
 import com.kouros.navigation.utils.GeoUtils.createPointCollection
+import com.kouros.navigation.utils.location
 import org.junit.Before
 import org.junit.Test
 import org.maplibre.geojson.FeatureCollection
@@ -39,7 +40,7 @@ class RouteModelTest {
         waypointIndex: Int = 0,
     ): Step {
         val waypoints = (0 until numWaypoints).map { i ->
-            listOf(11.0 + index * 0.01 + i * 0.001, 48.0)
+            location(11.0 + index * 0.01 + i * 0.001, 48.0)
         }
         return Step(
             index = index,