RouteCalculator

2026-05-06 19:49:43 +02:00
parent 25ba7d7bf6
commit 8b612d5b80
9 changed files with 451 additions and 41 deletions
@@ -4,6 +4,7 @@ import android.location.Location
 import android.location.LocationManager
 import androidx.test.platform.app.InstrumentationRegistry
 import androidx.test.ext.junit.runners.AndroidJUnit4
+import com.kouros.navigation.car.navigation.Simulation
 import com.kouros.navigation.data.Constants.homeHohenwaldeck
 import com.kouros.navigation.data.RouteEngine
 import com.kouros.navigation.data.overpass.Overpass
@@ -11,6 +12,7 @@ import com.kouros.navigation.data.route.ManeuverType
 import com.kouros.navigation.data.tomtom.TomTomRepository
 import com.kouros.navigation.model.NavigationViewModel
 import com.kouros.navigation.model.RouteModel
+import com.kouros.navigation.utils.GeoUtils.snapLocation
 import com.kouros.navigation.utils.getSettingsRepository
 import com.kouros.navigation.utils.location
 import kotlinx.coroutines.delay
@@ -21,6 +23,8 @@ import org.junit.runner.RunWith

 import org.junit.Assert.*
 import org.junit.Before
+import org.maplibre.compose.expressions.dsl.step
+import org.maplibre.geojson.Point
 import kotlin.collections.forEach

 /**
@@ -33,6 +37,348 @@ class RouteModelTest {

    val routeModel = RouteModel()
    val location = Location(LocationManager.GPS_PROVIDER)
+
+    val leftDistance = listOf(
+        64.0,
+        0.0,
+        143.7469482421875,
+        111.36382293701172,
+        101.35636901855469,
+        0.0,
+        356.0272216796875,
+        250.2962646484375,
+        245.09136962890625,
+        213.86209106445312,
+        173.69473266601562,
+        90.41661834716797,
+        40.58605194091797,
+        22.74074363708496,
+        12.33098030090332,
+        0.0,
+        1046.3043212890625,
+        1026.2342529296875,
+        1012.8703002929688,
+        979.4789428710938,
+        972.7660522460938,
+        971.6541137695312,
+        914.8672485351562,
+        873.6181030273438,
+        831.3056030273438,
+        781.1799926757812,
+        719.982666015625,
+        653.1173095703125,
+        578.4839477539062,
+        566.2300415039062,
+        490.4866943359375,
+        482.66766357421875,
+        451.45355224609375,
+        443.6345520019531,
+        391.28887939453125,
+        346.71197509765625,
+        316.65283203125,
+        237.5792236328125,
+        141.71861267089844,
+        78.22917175292969,
+        55.94073486328125,
+        41.31438064575195,
+        31.061260223388672,
+        20.943891525268555,
+        16.43439483642578,
+        0.0,
+        346.5630798339844,
+        339.0434875488281,
+        329.3117370605469,
+        321.055908203125,
+        304.0730285644531,
+        285.6786193847656,
+        272.10870361328125,
+        233.37289428710938,
+        213.9094696044922,
+        198.87030029296875,
+        164.74563598632812,
+        136.4317626953125,
+        91.64434051513672,
+        82.11692810058594,
+        60.832786560058594,
+        33.411705017089844,
+        0.0,
+        338.5975341796875,
+        318.1346435546875,
+        211.02232360839844,
+        201.35411071777344,
+        187.75144958496094,
+        179.0858154296875,
+        170.42019653320312,
+        123.4335708618164,
+        90.03872680664062,
+        74.55345916748047,
+        60.95069122314453,
+        39.9212532043457,
+        0.0,
+        314.19586181640625,
+        306.1593017578125,
+        289.4617004394531,
+        223.2646484375,
+        129.7374725341797,
+        76.47386932373047,
+        56.635841369628906,
+        52.30287170410156,
+        33.61341857910156,
+        20.613277435302734,
+        0.0,
+        5621.87744140625,
+        5612.591796875,
+        5587.8486328125,
+        5483.76806640625,
+        5447.275390625,
+        5422.7744140625,
+        5398.21875,
+        5373.927734375,
+        5358.28955078125,
+        5347.1455078125,
+        5339.36181640625,
+        5314.6181640625,
+        5303.107421875,
+        5278.109375,
+        5253.859375,
+        5244.88720703125,
+        5200.0400390625,
+        5151.9326171875,
+        5138.63916015625,
+        5085.87548828125,
+        5054.32080078125,
+        5012.51220703125,
+        4962.390625,
+        4938.140625,
+        4898.97607421875,
+        4862.1416015625,
+        4788.78564453125,
+        4767.880859375,
+        4761.89013671875,
+        4756.5390625,
+        4749.23388671875,
+        4724.98388671875,
+        4645.017578125,
+        4596.826171875,
+        4573.98583984375,
+        4554.85009765625,
+        4521.48388671875,
+        4499.232421875,
+        4484.6064453125,
+        4471.07763671875,
+        4457.54931640625,
+        4415.10986328125,
+        4389.8359375,
+        4359.6376953125,
+        4347.59716796875,
+        4342.62109375,
+        4334.59375,
+        4320.33447265625,
+        4303.408203125,
+        4289.998046875,
+        4261.86962890625,
+        4215.78515625,
+        4207.08251953125,
+        4192.81201171875,
+        4080.384521484375,
+        4065.3955078125,
+        4009.8798828125,
+        3975.99853515625,
+        3971.5283203125,
+        3953.9345703125,
+        3915.152587890625,
+        3877.74365234375,
+        3865.702880859375,
+        3857.67578125,
+        3850.37060546875,
+        3845.019287109375,
+        3843.161865234375,
+        3819.26904296875,
+        3803.01171875,
+        3686.892578125,
+        3661.30908203125,
+        3629.2001953125,
+        3613.607177734375,
+        3591.255859375,
+        3580.988525390625,
+        3573.37060546875,
+        3563.377685546875,
+        3552.08349609375,
+        3537.639404296875,
+        3502.25146484375,
+        3484.319580078125,
+        3445.313720703125,
+        3437.883544921875,
+        3431.69873046875,
+        3413.72021484375,
+        3405.05224609375,
+        3340.725830078125,
+        3312.782470703125,
+        3302.888671875,
+        3292.994873046875,
+        3240.986328125,
+        3229.830322265625,
+        3185.277099609375,
+        3096.74755859375,
+        2981.00146484375,
+        2907.40869140625,
+        2865.943115234375,
+        2859.75830078125,
+        2851.719482421875,
+        2813.998779296875,
+        2778.027587890625,
+        2767.468505859375,
+        2729.052001953125,
+        2694.80712890625,
+        2651.47265625,
+        2613.131591796875,
+        2602.36376953125,
+        2588.665771484375,
+        2549.015380859375,
+        2530.110107421875,
+        2521.1689453125,
+        2510.401123046875,
+        2478.26953125,
+        2437.71337890625,
+        2400.48681640625,
+        2350.667236328125,
+        2320.4267578125,
+        2294.857666015625,
+        2268.75830078125,
+        2260.833740234375,
+        2249.615234375,
+        2229.476318359375,
+        2210.573486328125,
+        2192.78271484375,
+        2184.963623046875,
+        2148.23974609375,
+        2114.8818359375,
+        2105.955078125,
+        2052.49951171875,
+        2013.5179443359375,
+        2006.804931640625,
+        1959.955810546875,
+        1869.44775390625,
+        1787.787109375,
+        1740.873046875,
+        1719.6939697265625,
+        1554.079833984375,
+        1395.2606201171875,
+        1349.453369140625,
+        1296.93359375,
+        1144.57080078125,
+        1030.08203125,
+        972.0895385742188,
+        926.4032592773438,
+        921.95556640625,
+        911.9205322265625,
+        902.9940185546875,
+        887.3560180664062,
+        803.21728515625,
+        774.1542358398438,
+        767.318603515625,
+        762.62841796875,
+        754.703857421875,
+        751.3680419921875,
+        738.9346923828125,
+        725.406005859375,
+        710.6475219726562,
+        668.3836669921875,
+        615.11279296875,
+        605.7323608398438,
+        572.6571044921875,
+        552.6024780273438,
+        545.567138671875,
+        474.7272644042969,
+        459.36187744140625,
+        445.29119873046875,
+        427.3172607421875,
+        406.21124267578125,
+        378.6969909667969,
+        371.8613586425781,
+        362.934814453125,
+        347.29681396484375,
+        329.3663330078125,
+        320.4398193359375,
+        301.3039855957031,
+        237.61085510253906,
+        112.25553131103516,
+        94.07954406738281,
+        75.73666381835938,
+        0.0,
+        1023.4342651367188,
+        1003.37939453125,
+        973.33203125,
+        901.591796875,
+        805.4495849609375,
+        776.5214233398438,
+        723.6364135742188,
+        713.1954956054688,
+        693.5875854492188,
+        646.0399169921875,
+        565.5601196289062,
+        509.7200622558594,
+        484.1024169921875,
+        422.78350830078125,
+        276.693603515625,
+        265.35223388671875,
+        187.42837524414062,
+        160.74212646484375,
+        97.36228942871094,
+        29.534751892089844,
+        18.390586853027344,
+        0.0,
+        462.20318603515625,
+        450.08526611328125,
+        439.66217041015625,
+        397.6007080078125,
+        346.669921875,
+        290.0765380859375,
+        270.7193603515625,
+        208.91537475585938,
+        191.0126495361328,
+        177.56549072265625,
+        165.6016082763672,
+        117.09382629394531,
+        108.90425109863281,
+        75.23577117919922,
+        37.74562072753906,
+        0.0,
+        940.9969482421875,
+        921.5716552734375,
+        891.173583984375,
+        876.2035522460938,
+        865.4982299804688,
+        851.6742553710938,
+        808.6068725585938,
+        793.0664672851562,
+        757.3865356445312,
+        716.390380859375,
+        701.561767578125,
+        696.2328491210938,
+        657.3816528320312,
+        641.1264038085938,
+        584.9859619140625,
+        581.8070678710938,
+        552.8590087890625,
+        473.6039733886719,
+        401.5708923339844,
+        369.44659423828125,
+        360.9601745605469,
+        353.1898498535156,
+        309.3813781738281,
+        269.4887390136719,
+        226.76416015625,
+        141.3342742919922,
+        135.6678466796875,
+        126.8246078491211,
+        94.33628845214844,
+        68.04391479492188,
+        0.0,
+        149.6364288330078,
+        118.28071594238281,
+        0.0,
+    )
    val distance = listOf(
        1046.5,
        1026.5,
@@ -203,30 +549,20 @@ class RouteModelTest {

    @Test
    fun simulate() {
-        val viewModel = NavigationViewModel(TomTomRepository())
        for ((index, waypoint) in routeModel.curRoute.waypoints.withIndex()) {
            if (routeModel.isNavigating()) {
                val curLocation = location(waypoint[0], waypoint[1])
                if (index in 0..routeModel.curRoute.waypoints.size) {
-                    //runBlocking { delay(1000) }
-                    val start = System.currentTimeMillis()
                    routeModel.updateLocation(curLocation, NavigationViewModel(TomTomRepository()))
                    val stepData = routeModel.currentStep()
-                    //println("${stepData.instruction} ${System.currentTimeMillis() - start}")
-                    if (stepData.lane.isNotEmpty()) {
-                        // println(stepData.street)
-                        stepData.lane.forEach {
-                            // println("${it.indications} ${it.valid}")
-                        }
-                    }
-                    // val nextData =  routeModel.nextStep()
+                    assertEquals(stepData.leftStepDistance, leftDistance[index], 1.0)
                }
            }
        }
    }

    @Test
-    fun `leftStepDistance Inglolstädter  `() {
+    fun `leftStepDistance Ingolstädter  `() {
        var location = location(11.584352, 48.186771)
        routeModel.updateLocation(location, NavigationViewModel(TomTomRepository()))
        var step = routeModel.currentStep()
@@ -264,5 +600,29 @@ class RouteModelTest {
        val end = System.currentTimeMillis() - start
        println("Time $end")
    }
+
+    @Test
+    fun `check leftStepDistance Vogelhart Gpx  `() {
+        routeModel.navState = routeModel.navState.copy(routeBearing = 270F)
+        var location = location(11.57927955, 48.18554854)
+        routeModel.updateLocation(location, NavigationViewModel(TomTomRepository()))
+        var stepData = routeModel.currentStep()
+        assertEquals(stepData.leftStepDistance, 62.0, 1.0)
+
+        location = location(11.57919950, 48.18556317)
+        routeModel.updateLocation(location, NavigationViewModel(TomTomRepository()))
+        stepData = routeModel.currentStep()
+        assertEquals(stepData.leftStepDistance, 58.0, 1.0)
+
+        location = location(11.57906832, 48.18556405)
+        routeModel.updateLocation(location, NavigationViewModel(TomTomRepository()))
+        stepData = routeModel.currentStep()
+        assertEquals(stepData.leftStepDistance, 48.0, 1.0)
+
+        routeModel.navState = routeModel.navState.copy(routeBearing = 270F)
+        location = location(11.57875945, 48.18558161)
+        routeModel.updateLocation(location, NavigationViewModel(TomTomRepository()))
+        //assertEquals(routeModel.currentStep().leftStepDistance, 0.0, 1.0)
+    }
 }

@@ -28,8 +28,8 @@ class Simulation {
    ) {
        if (routeModel.navState.route.isRouteValid()) {
            if (BuildConfig.DEBUG) {
-                //gpxSimulation(routeModel, lifecycleScope, updateLocation)
-                currentSimulation(routeModel, lifecycleScope, updateLocation)
+                gpxSimulation(routeModel, lifecycleScope, updateLocation)
+                //currentSimulation(routeModel, lifecycleScope, updateLocation)
            } else {
                currentSimulation(routeModel, lifecycleScope, updateLocation)
            }
@@ -81,7 +81,7 @@ class Simulation {
        runBlocking {
            simulationJob = launch(Dispatchers.IO) {
                route = TomTomRepository().fetchUrl(
-                    "https://kouros-online.de/vh.gpx",
+                    "https://kouros-online.de/VH.gpx",
                    false
                )
            }
@@ -121,7 +121,7 @@ class Simulation {
                            if (duration > 100) {
                             //   delay(duration / 4)
                            }
-                            delay(500)
+                            delay(200)
                            lastTime = p.time
                            lastLocation = fakeLocation
                        }
@@ -3,15 +3,16 @@ package com.kouros.navigation.car.screen
 import androidx.car.app.CarContext
 import androidx.car.app.Screen
 import androidx.car.app.model.Action
+import androidx.car.app.model.CarColor
 import androidx.car.app.model.CarIcon
 import androidx.car.app.model.GridItem
 import androidx.car.app.model.GridTemplate
 import androidx.car.app.model.Header
 import androidx.car.app.model.ItemList
-import androidx.car.app.model.ListTemplate
-import androidx.car.app.model.Row
 import androidx.car.app.model.Template
+import androidx.compose.ui.graphics.Color
 import androidx.core.graphics.drawable.IconCompat
+import androidx.core.graphics.toColorInt
 import com.kouros.data.R
 import com.kouros.navigation.car.SurfaceRenderer
 import com.kouros.navigation.data.Category
@@ -80,15 +81,50 @@ class CategoriesScreen(
    }
 }

+
 fun carIcon(context: CarContext, category: String, index: Int): CarIcon {
+
+    val customCarColor =
+        CarColor.createCustom(android.graphics.Color.MAGENTA, android.graphics.Color.MAGENTA)
+
    if (index == -1) {
-        val resId = when (category) {
-            CHARGING_STATION -> R.drawable.ev_station_24px
-            FUEL_STATION -> R.drawable.local_gas_station_24
-            PHARMACY -> R.drawable.local_pharmacy_24px
-            else -> R.drawable.restaurant_24px
+        val icon = when (category) {
+            CHARGING_STATION -> CarIcon.Builder(
+                IconCompat.createWithResource(
+                    context,
+                    R.drawable.ev_station_24px
+                )
+            ).setTint(CarColor.GREEN)
+
+            FUEL_STATION -> CarIcon.Builder(
+                IconCompat.createWithResource(
+                    context,
+                    R.drawable.local_gas_station_24
+                )
+            ).setTint(CarColor.BLUE)
+
+            PHARMACY -> CarIcon.Builder(
+                IconCompat.createWithResource(
+                    context,
+                    R.drawable.local_pharmacy_24px
+                )
+            ).setTint(CarColor.RED)
+
+            RESTAURANT -> CarIcon.Builder(
+                IconCompat.createWithResource(
+                    context,
+                    R.drawable.restaurant_24px
+                )
+            ).setTint(customCarColor)
+
+            else -> CarIcon.Builder(
+                IconCompat.createWithResource(
+                    context,
+                    R.drawable.traffic_jam_48px
+                )
+            ).setTint(CarColor.YELLOW)
        }
-        return CarIcon.Builder(IconCompat.createWithResource(context, resId)).build()
+        return icon.build()
    } else {
        return CarIcon.Builder(
            createNumberIcon(
@@ -222,10 +222,9 @@ open class NavigationScreen(
        return NavigationTemplate.Builder()
            .setNavigationInfo(
                MessageInfo.Builder(
-                    message
-                //carContext.getString(R.string.arrived_exclamation_msg)
+                carContext.getString(R.string.arrived_exclamation_msg)
                )
-                   // .setText(street)
+                    .setText(message)
                    .setImage(
                        CarIcon.Builder(
                            IconCompat.createWithResource(
@@ -76,7 +76,10 @@ class PlaceListScreen(
        val itemListBuilder = ItemList.Builder()
            .setNoItemsMessage(carContext.getString(R.string.no_places))
        recentPlaces.filter { it.category == category }.forEach {
-            val street = it.street
+            val street = it.street.ifEmpty {
+                it.name
+            }
+
            val row = Row.Builder()
                .setImage(contactIcon(null, it.category))
                .setTitle("$street ${it.city}")
@@ -21,7 +21,7 @@ const val tomtomTrafficUrl = "https://api.tomtom.com/traffic/services/5/incident
 private const val tomtomFields =
    "{incidents{type,geometry{type,coordinates},properties{iconCategory,events{description}}}}"

-val useLocal =   BuildConfig.DEBUG
+val useLocal =  BuildConfig.DEBUG

 val useLocalTraffic = BuildConfig.DEBUG

@@ -64,10 +64,10 @@ class TomTomRoute {
                    // calculate left step distance for each point
                    var leftStepDistance = stepDistance
                    leftDistance.add(leftStepDistance.toFloat())
-                    points.subList(
+                    val subPoints = points.subList(
                        lastPointIndex,
-                        instruction.pointIndex + 1,
-                    ).forEach {
+                        instruction.pointIndex + 1)
+                    subPoints.forEach {
                        if (lastLocation.latitude != 0.0) {
                            val curLocation = location(it[0], it[1])
                            val dist = curLocation.distanceTo(lastLocation).absoluteValue
@@ -80,10 +80,7 @@ class TomTomRoute {
                        bearingBefore = 0,
                        bearingAfter = 0,
                        type = convertType(instruction.maneuver),
-                        waypoints = points.subList(
-                            lastPointIndex,
-                            instruction.pointIndex + 1,
-                        ).map { location(it[0], it[1]) },
+                        waypoints = subPoints.map { location(it[0], it[1]) },
                        exit = exitNumber(instruction),
                        location = location(
                            instruction.point.longitude, instruction.point.latitude
@@ -8,6 +8,7 @@ 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.utils.location
 import java.util.concurrent.TimeUnit
 import kotlin.math.absoluteValue
@@ -20,28 +21,42 @@ class RouteCalculator(var routeModel: RouteModel) {

    fun findStep(location: Location) {
        var nearestDistance = MAXIMUM_LOCATION_DISTANCE
+        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) {
+                        if (distance < nearestDistance  ) {
                            nearestDistance = distance
                            routeModel.navState.route.currentStepIndex = step.index
                            step.waypointIndex = wayIndex
                            step.wayPointLocation = waypoint
                        }
                    }
-                    if (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 5) {
+                    if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
                        break
                    }
+                    if (distance > lastDistance) {
+                        increaseDistance++
+                    }
+                    lastDistance = distance
                }
            }
-            if (nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 5) {
+            if (stopSearch(nearestDistance, distance) || increaseDistance > 20) {
                break
            }
        }
+        Log.d(TAG, "FindStep $nearestDistance $count $increaseDistance")
+    }
+
+    private fun stopSearch(nearestDistance: Float, distance: Float): Boolean {
+        return nearestDistance < NEAREST_LOCATION_DISTANCE && distance > NEAREST_LOCATION_DISTANCE * 10
    }

    fun travelLeftTime(): Double {
@@ -1,9 +1,9 @@
 [versions]
-agp = "9.2.0"
+agp = "9.2.1"
 androidGpxParser = "2.3.1"
 androidSdkTurf = "6.0.1"
 datastore = "1.2.1"
-gradle = "9.2.0"
+gradle = "9.2.1"
 koinAndroid = "4.2.1"
 koinAndroidxCompose = "4.2.1"
 koinComposeViewmodel = "4.2.1"