FindStep

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

@@ -5,6 +5,8 @@ import androidx.compose.ui.graphics.Color
 val NavigationColorLight = Color(0xFF17A119)
 val NavigationColorDark = Color(0xFF2B007A)
 
+val NavigationCircle = Color(0xFFFFEB3B)
+
 val RouteColor = Color(0xFF5201B4)
 
 val SpeedColor = Color(0xFF262525)

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

@@ -132,7 +132,7 @@ object Constants {
     val homeHohenwaldeck = location( 11.594322, 48.1164817)
     val ioannina = location( 20.826237, 39.690174)
     val subislawa = location(18.570808, 54.420647)
-    val a94 = location(11.872097,48.163449)
+    val a94 = location(11.947101,48.213251)
     val a9 = location(11.621556, 48.204402,)
 
     val a22 = location(10.845749, 45.602507)
@@ -141,7 +141,7 @@ object Constants {
 
     const val MAXIMAL_SNAP_CORRECTION = 50.0
 
-    const val MAXIMAL_ROUTE_DEVIATION = 100.0
+    const val MAXIMAL_ROUTE_DEVIATION = 150.0
 
     const val DESTINATION_ARRIVAL_DISTANCE = 10.0
 

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

@@ -22,62 +22,42 @@ class RouteCalculator(var routeModel: RouteModel) {
     var lastSpeedIndex: Int = 0
 
     fun findStep(location: Location) {
-        val route = routeModel.navState.route
-        val steps = routeModel.curLeg.steps
-        // Search from one step prior to handle deviations/U-turns
-        val startIndex = (route.currentStepIndex - 1).coerceAtLeast(0)
-        val endIndex = steps.size - 1
-
         var nearestDistance = MAXIMUM_LOCATION_DISTANCE
-        var searchCount = 0
-
-        for (i in startIndex..endIndex) {
-            val step = steps[i]
-            val waypoints = step.maneuver.waypoints
-            // Only offset search if we are exactly on the current stored step index
-            val startWayIndex = if (i == route.currentStepIndex) step.waypointIndex else 0
-
-            var lastDistance = Float.MAX_VALUE
-            var increaseCount = 0
-
-            for (j in startWayIndex until waypoints.size) {
-                searchCount++
-                val waypoint = waypoints[j]
-                val distance = location.distanceTo(waypoint)
-
-                if (distance < nearestDistance) {
-                    nearestDistance = distance
-                    bestMatch = StepMatch(step.index, j, waypoint)
-                }
-
-                if (distance > lastDistance) {
-                    increaseCount++
-                } else {
-                    increaseCount = 0
-                }
-                
-                if (stopSearch(nearestDistance, distance, increaseCount)) {
-                    bestMatch?.let { match ->
-                        route.currentStepIndex = match.stepIndex
-                        steps[match.stepIndex].waypointIndex = match.waypointIndex
-                        steps[match.stepIndex].wayPointLocation = match.location
+        var count = 0
+        var lastDistance = 0F
+        var increaseDistance = 0
+        for ((index, step) in routeModel.curLeg.steps.withIndex()) {
+            count++
+            var distance = 0F
+            if (index >= routeModel.navState.route.currentStepIndex) {
+                for ((wayIndex, waypoint) in step.maneuver.waypoints.withIndex()) {
+                    count++
+                    if (wayIndex >= step.waypointIndex) {
+                        distance = location.distanceTo(waypoint)
+                        if (distance < nearestDistance  ) {
+                            nearestDistance = distance
+                            routeModel.navState.route.currentStepIndex = step.index
+                            step.waypointIndex = wayIndex
+                            step.wayPointLocation = waypoint
+                        }
                     }
-                    return
+                    if (stopSearch(nearestDistance, distance)) {
+                        break
+                    }
+                    if (distance > lastDistance) {
+                        increaseDistance++
+                    }
+                    lastDistance = distance
                 }
-                lastDistance = distance
             }
-        }
-        bestMatch?.let { match ->
-            route.currentStepIndex = match.stepIndex
-            steps[match.stepIndex].waypointIndex = match.waypointIndex
-            steps[match.stepIndex].wayPointLocation = match.location
+            if (stopSearch(nearestDistance, distance)) {
+                break
+            }
         }
     }
 
-    private fun stopSearch(nearestDistance: Float, distance: Float, increaseCount: Int): Boolean {
-        // Increased multiplier from 10 to 20 to be less aggressive in sharp curves
-        return (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 20)
-                || increaseCount > 15
+    private fun stopSearch(nearestDistance: Float, distance: Float): Boolean {
+        return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10
     }
 
     fun travelLeftTime(): Double {

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

@@ -1,10 +1,12 @@
 package com.kouros.navigation.model
 
 import android.location.Location
+import android.util.Log
 import androidx.car.app.connection.CarConnection.CONNECTION_TYPE_NATIVE
 import androidx.car.app.connection.CarConnection.CONNECTION_TYPE_PROJECTION
 import androidx.car.app.navigation.model.Maneuver
 import com.kouros.navigation.data.Constants.NEXT_STEP_THRESHOLD
+import com.kouros.navigation.data.Constants.TAG
 import com.kouros.navigation.data.NavigationState
 import com.kouros.navigation.data.Route
 import com.kouros.navigation.data.StepData

common/data/src/main/java/com/kouros/navigation/utils/GeoUtils.kt

@@ -254,7 +254,7 @@ object GeoUtils {
     fun movePoint(
         latitude: Double,
         longitude: Double,
-        distanceMeters: Double = 8.0,
+        distanceMeters: Double = 5.0,
         bearingDegrees: Double = 0.0
     ): org.maplibre.spatialk.geojson.Point {
         val radius = EARTH_RADIUS * 1000 // meters

common/data/src/main/java/com/kouros/navigation/utils/NavigationUtils.kt

@@ -49,7 +49,7 @@ object NavigationUtils {
 }
 
 fun calculateZoom(speed: Double?): Double {
-    val zoom = 16.5
+    val zoom = 17.0
     if (speed == null) {
         return zoom
     }