feat: implement isochrone-based weather routing (Section 3.4)

5 files changed, 293 insertions, 0 deletions

diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt
new file mode 100644
index 0000000..60a5918
--- /dev/null
+++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneResult.kt
@@ -0,0 +1,12 @@
+package com.example.androidapp.routing
+
+/**
+ * The result of an isochrone weather routing computation.
+ *
+ * @param path  Ordered list of [RoutePoint]s from the start to the destination.
+ * @param etaMs Estimated Time of Arrival as a UNIX timestamp in milliseconds.
+ */
+data class IsochroneResult(
+    val path: List<RoutePoint>,
+    val etaMs: Long
+)
diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt
new file mode 100644
index 0000000..25055a8
--- /dev/null
+++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/IsochroneRouter.kt
@@ -0,0 +1,178 @@
+package com.example.androidapp.routing
+
+import com.example.androidapp.data.model.BoatPolars
+import com.example.androidapp.data.model.WindForecast
+import kotlin.math.asin
+import kotlin.math.atan2
+import kotlin.math.cos
+import kotlin.math.pow
+import kotlin.math.sin
+import kotlin.math.sqrt
+
+/**
+ * Isochrone-based weather routing engine (Section 3.4).
+ *
+ * Algorithm:
+ *  1. Start from a single point; expand a fan of headings at each time step.
+ *  2. For each candidate heading, compute BSP from [BoatPolars] at the local forecast wind.
+ *  3. Advance position by BSP × Δt using the spherical-Earth destination-point formula.
+ *  4. Check whether the destination has been reached (within [arrivalRadiusM]).
+ *  5. Prune candidates: for each angular sector around the start, keep only the point that
+ *     advanced furthest (removes dominated points).
+ *  6. Repeat until the destination is reached or [maxSteps] is exhausted.
+ *  7. Backtrace parent pointers to produce the optimal path.
+ */
+object IsochroneRouter {
+
+    private const val EARTH_RADIUS_M = 6_371_000.0
+    internal const val NM_TO_M = 1_852.0
+    private const val KT_TO_M_PER_S = NM_TO_M / 3600.0
+
+    const val DEFAULT_HEADING_STEP_DEG = 5.0
+    const val DEFAULT_ARRIVAL_RADIUS_M = 1_852.0   // 1 NM
+    const val DEFAULT_PRUNE_SECTORS = 72           // 5° sectors
+    const val DEFAULT_MAX_STEPS = 200
+
+    /**
+     * Compute an optimised route from start to destination.
+     *
+     * @param startLat        Start latitude (decimal degrees).
+     * @param startLon        Start longitude (decimal degrees).
+     * @param destLat         Destination latitude (decimal degrees).
+     * @param destLon         Destination longitude (decimal degrees).
+     * @param startTimeMs     Departure time as UNIX timestamp (ms).
+     * @param stepMs          Time increment per isochrone step (ms). Typical: 1–3 hours.
+     * @param polars          Boat polar table.
+     * @param windAt          Function returning [WindForecast] for a given position and time.
+     * @param headingStepDeg  Angular resolution of the heading fan (degrees). Default 5°.
+     * @param arrivalRadiusM  Distance threshold to consider destination reached (metres).
+     * @param maxSteps        Maximum number of isochrone expansions before giving up.
+     * @return [IsochroneResult] with the optimal path and ETA, or null if unreachable.
+     */
+    fun route(
+        startLat: Double,
+        startLon: Double,
+        destLat: Double,
+        destLon: Double,
+        startTimeMs: Long,
+        stepMs: Long,
+        polars: BoatPolars,
+        windAt: (lat: Double, lon: Double, timeMs: Long) -> WindForecast,
+        headingStepDeg: Double = DEFAULT_HEADING_STEP_DEG,
+        arrivalRadiusM: Double = DEFAULT_ARRIVAL_RADIUS_M,
+        maxSteps: Int = DEFAULT_MAX_STEPS
+    ): IsochroneResult? {
+        val start = RoutePoint(startLat, startLon, startTimeMs)
+        var isochrone = listOf(start)
+
+        repeat(maxSteps) { step ->
+            val nextTimeMs = startTimeMs + (step + 1).toLong() * stepMs
+            val candidates = mutableListOf<RoutePoint>()
+
+            for (point in isochrone) {
+                var heading = 0.0
+                while (heading < 360.0) {
+                    val wind = windAt(point.lat, point.lon, point.timestampMs)
+                    val twa = ((heading - wind.twdDeg + 360.0) % 360.0)
+                    val bspKt = polars.bsp(twa, wind.twsKt)
+                    if (bspKt > 0.0) {
+                        val distM = bspKt * KT_TO_M_PER_S * (stepMs / 1000.0)
+                        val (newLat, newLon) = destinationPoint(point.lat, point.lon, heading, distM)
+                        val newPoint = RoutePoint(newLat, newLon, nextTimeMs, parent = point)
+
+                        if (haversineM(newLat, newLon, destLat, destLon) <= arrivalRadiusM) {
+                            return IsochroneResult(
+                                path = backtrace(newPoint),
+                                etaMs = nextTimeMs
+                            )
+                        }
+                        candidates.add(newPoint)
+                    }
+                    heading += headingStepDeg
+                }
+            }
+
+            if (candidates.isEmpty()) return null
+            isochrone = prune(candidates, startLat, startLon, DEFAULT_PRUNE_SECTORS)
+        }
+
+        return null
+    }
+
+    /** Walk parent pointers from destination back to start, then reverse. */
+    internal fun backtrace(dest: RoutePoint): List<RoutePoint> {
+        val path = mutableListOf<RoutePoint>()
+        var current: RoutePoint? = dest
+        while (current != null) {
+            path.add(current)
+            current = current.parent
+        }
+        path.reverse()
+        return path
+    }
+
+    /**
+     * Angular-sector pruning: divide the plane into [sectors] equal angular sectors around the
+     * start. Within each sector keep only the candidate that is furthest from the start.
+     */
+    internal fun prune(
+        candidates: List<RoutePoint>,
+        startLat: Double,
+        startLon: Double,
+        sectors: Int
+    ): List<RoutePoint> {
+        val sectorSize = 360.0 / sectors
+        val best = mutableMapOf<Int, RoutePoint>()
+
+        for (point in candidates) {
+            val bearing = bearingDeg(startLat, startLon, point.lat, point.lon)
+            val sector = (bearing / sectorSize).toInt().coerceIn(0, sectors - 1)
+            val existing = best[sector]
+            if (existing == null ||
+                haversineM(startLat, startLon, point.lat, point.lon) >
+                haversineM(startLat, startLon, existing.lat, existing.lon)
+            ) {
+                best[sector] = point
+            }
+        }
+
+        return best.values.toList()
+    }
+
+    /** Haversine great-circle distance in metres. */
+    internal fun haversineM(lat1: Double, lon1: Double, lat2: Double, lon2: Double): Double {
+        val dLat = Math.toRadians(lat2 - lat1)
+        val dLon = Math.toRadians(lon2 - lon1)
+        val a = sin(dLat / 2).pow(2) +
+                cos(Math.toRadians(lat1)) * cos(Math.toRadians(lat2)) * sin(dLon / 2).pow(2)
+        return 2.0 * EARTH_RADIUS_M * asin(sqrt(a))
+    }
+
+    /** Initial bearing from point 1 to point 2 (degrees, 0 = North, clockwise). */
+    internal fun bearingDeg(lat1Deg: Double, lon1Deg: Double, lat2Deg: Double, lon2Deg: Double): Double {
+        val lat1 = Math.toRadians(lat1Deg)
+        val lat2 = Math.toRadians(lat2Deg)
+        val dLon = Math.toRadians(lon2Deg - lon1Deg)
+        val y = sin(dLon) * cos(lat2)
+        val x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon)
+        return (Math.toDegrees(atan2(y, x)) + 360.0) % 360.0
+    }
+
+    /** Spherical-Earth destination-point given start, bearing, and distance. */
+    internal fun destinationPoint(
+        lat1Deg: Double,
+        lon1Deg: Double,
+        bearingDeg: Double,
+        distM: Double
+    ): Pair<Double, Double> {
+        val lat1 = Math.toRadians(lat1Deg)
+        val lon1 = Math.toRadians(lon1Deg)
+        val brng = Math.toRadians(bearingDeg)
+        val d = distM / EARTH_RADIUS_M
+
+        val lat2 = asin(sin(lat1) * cos(d) + cos(lat1) * sin(d) * cos(brng))
+        val lon2 = lon1 + atan2(sin(brng) * sin(d) * cos(lat1), cos(d) - sin(lat1) * sin(lat2))
+
+        return Pair(Math.toDegrees(lat2), Math.toDegrees(lon2))
+    }
+}
diff --git a/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt b/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt
new file mode 100644
index 0000000..02988d1
--- /dev/null
+++ b/android-app/app/src/main/kotlin/com/example/androidapp/routing/RoutePoint.kt
@@ -0,0 +1,16 @@
+package com.example.androidapp.routing
+
+/**
+ * A single point in the isochrone routing tree.
+ *
+ * @param lat         Latitude (decimal degrees).
+ * @param lon         Longitude (decimal degrees).
+ * @param timestampMs UNIX time in milliseconds when this position is reached.
+ * @param parent      The previous [RoutePoint] (null for the start point).
+ */
+data class RoutePoint(
+    val lat: Double,
+    val lon: Double,
+    val timestampMs: Long,
+    val parent: RoutePoint? = null
+)
diff --git a/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt b/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt
new file mode 100644
index 0000000..0286ea8
--- /dev/null
+++ b/android-app/app/src/main/kotlin/org/terst/nav/data/model/BoatPolars.kt
@@ -0,0 +1,69 @@
+package org.terst.nav.data.model
+
+import kotlin.math.pow
+import kotlin.math.sqrt
+
+/**
+ * Boat polar speed table: maps (TWA, TWS) → BSP (boat speed through water, knots).
+ *
+ * Interpolation is bilinear — linear on TWA within a given TWS, then linear on TWS.
+ * Port-tack mirror: TWA > 180° is folded to 360° - TWA before lookup.
+ */
+data class BoatPolars(
+    /** Outer key: TWS in knots. Inner key: TWA in degrees [0, 180]. Value: BSP in knots. */
+    val table: Map<Double, Map<Double, Double>>
+) {
+    /**
+     * Returns boat speed (knots) for the given True Wind Angle and True Wind Speed.
+     * TWA outside [0, 360] is clamped; port/starboard symmetry is applied (>180° mirrored).
+     */
+    fun bsp(twaDeg: Double, twsKt: Double): Double {
+        val twa = if (twaDeg > 180.0) 360.0 - twaDeg else twaDeg.coerceIn(0.0, 180.0)
+
+        val twsKeys = table.keys.sorted()
+        if (twsKeys.isEmpty()) return 0.0
+
+        val twsClamped = twsKt.coerceIn(twsKeys.first(), twsKeys.last())
+        val twsLow = twsKeys.lastOrNull { it <= twsClamped } ?: twsKeys.first()
+        val twsHigh = twsKeys.firstOrNull { it >= twsClamped } ?: twsKeys.last()
+
+        val bspLow = bspAtTws(twa, table[twsLow] ?: return 0.0)
+        val bspHigh = bspAtTws(twa, table[twsHigh] ?: return 0.0)
+
+        return if (twsHigh == twsLow) bspLow
+        else {
+            val t = (twsClamped - twsLow) / (twsHigh - twsLow)
+            bspLow + t * (bspHigh - bspLow)
+        }
+    }
+
+    private fun bspAtTws(twaDeg: Double, twaMap: Map<Double, Double>): Double {
+        val twaKeys = twaMap.keys.sorted()
+        if (twaKeys.isEmpty()) return 0.0
+
+        val twaClamped = twaDeg.coerceIn(twaKeys.first(), twaKeys.last())
+        val twaLow = twaKeys.lastOrNull { it <= twaClamped } ?: twaKeys.first()
+        val twaHigh = twaKeys.firstOrNull { it >= twaClamped } ?: twaKeys.last()
+
+        val bspLow = twaMap[twaLow] ?: 0.0
+        val bspHigh = twaMap[twaHigh] ?: 0.0
+
+        return if (twaHigh == twaLow) bspLow
+        else {
+            val t = (twaClamped - twaLow) / (twaHigh - twaLow)
+            bspLow + t * (bspHigh - bspLow)
+        }
+    }
+
+    companion object {
+        /** Default polar for a typical 35-foot cruising sloop. */
+        val DEFAULT: BoatPolars = BoatPolars(
+            mapOf(
+                5.0  to mapOf(45.0 to 3.5, 60.0 to 4.2, 90.0 to 4.8, 120.0 to 5.0, 150.0 to 4.5, 180.0 to 4.0),
+                10.0 to mapOf(45.0 to 5.5, 60.0 to 6.5, 90.0 to 7.0, 120.0 to 7.2, 150.0 to 6.8, 180.0 to 6.0),
+                15.0 to mapOf(45.0 to 6.5, 60.0 to 7.5, 90.0 to 8.0, 120.0 to 8.5, 150.0 to 8.0, 180.0 to 7.0),
+                20.0 to mapOf(45.0 to 7.0, 60.0 to 8.0, 90.0 to 8.5, 120.0 to 9.0, 150.0 to 8.5, 180.0 to 7.5)
+            )
+        )
+    }
+}
diff --git a/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt b/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt
new file mode 100644
index 0000000..f009da8
--- /dev/null
+++ b/android-app/app/src/main/kotlin/org/terst/nav/data/model/WindForecast.kt
@@ -0,0 +1,18 @@
+package org.terst.nav.data.model
+
+/**
+ * Wind conditions at a specific location and time.
+ *
+ * @param lat         Latitude (decimal degrees).
+ * @param lon         Longitude (decimal degrees).
+ * @param timestampMs UNIX time in milliseconds.
+ * @param twsKt       True Wind Speed in knots.
+ * @param twdDeg      True Wind Direction in degrees (the direction FROM which the wind blows, 0–360).
+ */
+data class WindForecast(
+    val lat: Double,
+    val lon: Double,
+    val timestampMs: Long,
+    val twsKt: Double,
+    val twdDeg: Double
+)