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

6 files changed, 462 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
+)
diff --git a/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt b/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt
new file mode 100644
index 0000000..e5615e9
--- /dev/null
+++ b/android-app/app/src/test/kotlin/com/example/androidapp/routing/IsochroneRouterTest.kt
@@ -0,0 +1,169 @@
+package com.example.androidapp.routing
+
+import com.example.androidapp.data.model.BoatPolars
+import com.example.androidapp.data.model.WindForecast
+import org.junit.Assert.*
+import org.junit.Test
+
+class IsochroneRouterTest {
+
+    private val startTimeMs = 1_000_000_000L
+    private val oneHourMs = 3_600_000L
+
+    // ── BoatPolars ────────────────────────────────────────────────────────────
+
+    @Test
+    fun `bsp returns exact value for exact twa and tws entry`() {
+        val polars = BoatPolars.DEFAULT
+        // At TWS=10, TWA=90 the table has 7.0 kt
+        assertEquals(7.0, polars.bsp(90.0, 10.0), 1e-9)
+    }
+
+    @Test
+    fun `bsp interpolates between twa entries`() {
+        val polars = BoatPolars.DEFAULT
+        // At TWS=10: TWA=60 → 6.5, TWA=90 → 7.0; midpoint TWA=75 → 6.75
+        assertEquals(6.75, polars.bsp(75.0, 10.0), 1e-9)
+    }
+
+    @Test
+    fun `bsp interpolates between tws entries`() {
+        val polars = BoatPolars.DEFAULT
+        // At TWA=90: TWS=10 → 7.0, TWS=15 → 8.0; midpoint TWS=12.5 → 7.5
+        assertEquals(7.5, polars.bsp(90.0, 12.5), 1e-9)
+    }
+
+    @Test
+    fun `bsp mirrors port tack twa to starboard`() {
+        val polars = BoatPolars.DEFAULT
+        // TWA=270 should mirror to 360-270=90, giving same as TWA=90
+        assertEquals(polars.bsp(90.0, 10.0), polars.bsp(270.0, 10.0), 1e-9)
+    }
+
+    @Test
+    fun `bsp clamps tws below table minimum`() {
+        val polars = BoatPolars.DEFAULT
+        // TWS=0 clamps to minimum TWS=5
+        assertEquals(polars.bsp(90.0, 5.0), polars.bsp(90.0, 0.0), 1e-9)
+    }
+
+    @Test
+    fun `bsp clamps tws above table maximum`() {
+        val polars = BoatPolars.DEFAULT
+        // TWS=100 clamps to maximum TWS=20
+        assertEquals(polars.bsp(90.0, 20.0), polars.bsp(90.0, 100.0), 1e-9)
+    }
+
+    // ── IsochroneRouter geometry helpers ─────────────────────────────────────
+
+    @Test
+    fun `haversineM returns zero for same point`() {
+        assertEquals(0.0, IsochroneRouter.haversineM(10.0, 20.0, 10.0, 20.0), 1e-3)
+    }
+
+    @Test
+    fun `haversineM one degree of latitude is approximately 111_195 m`() {
+        val dist = IsochroneRouter.haversineM(0.0, 0.0, 1.0, 0.0)
+        assertEquals(111_195.0, dist, 50.0)
+    }
+
+    @Test
+    fun `bearingDeg returns 0 for due north`() {
+        val bearing = IsochroneRouter.bearingDeg(0.0, 0.0, 1.0, 0.0)
+        assertEquals(0.0, bearing, 1e-6)
+    }
+
+    @Test
+    fun `bearingDeg returns 90 for due east`() {
+        val bearing = IsochroneRouter.bearingDeg(0.0, 0.0, 0.0, 1.0)
+        assertEquals(90.0, bearing, 1e-4)
+    }
+
+    @Test
+    fun `destinationPoint due north by 1 NM moves latitude by expected amount`() {
+        val (lat, lon) = IsochroneRouter.destinationPoint(0.0, 0.0, 0.0, IsochroneRouter.NM_TO_M)
+        assertTrue("latitude should increase", lat > 0.0)
+        assertEquals(0.0, lon, 1e-9)
+        // 1 NM ≈ 1/60 degree of latitude
+        assertEquals(1.0 / 60.0, lat, 1e-4)
+    }
+
+    // ── Pruning ───────────────────────────────────────────────────────────────
+
+    @Test
+    fun `prune keeps only furthest point per sector`() {
+        // Two points both due north of origin at different distances
+        val close = RoutePoint(1.0, 0.0, startTimeMs)
+        val far   = RoutePoint(2.0, 0.0, startTimeMs)
+        val result = IsochroneRouter.prune(listOf(close, far), 0.0, 0.0, 72)
+        assertEquals(1, result.size)
+        assertEquals(far, result[0])
+    }
+
+    @Test
+    fun `prune keeps points in different sectors separately`() {
+        // One point north, one point east — different sectors
+        val north = RoutePoint(1.0,  0.0, startTimeMs)
+        val east  = RoutePoint(0.0,  1.0, startTimeMs)
+        val result = IsochroneRouter.prune(listOf(north, east), 0.0, 0.0, 72)
+        assertEquals(2, result.size)
+    }
+
+    // ── Full routing ──────────────────────────────────────────────────────────
+
+    @Test
+    fun `route finds path to destination with constant wind`() {
+        // Destination is ~5 NM due east of start; constant 10kt easterly (FROM east = 90°)
+        // A 10kt boat sailing downwind (TWA=180) = 6.0 kt; ~5 NM / 6 kt ≈ 50 min → 1 step
+        val destLat = 0.0
+        val destLon = 0.0 + (5.0 / 60.0)  // ~5 NM east
+        val constantWind = { _: Double, _: Double, _: Long ->
+            WindForecast(0.0, 0.0, startTimeMs, twsKt = 10.0, twdDeg = 90.0)
+        }
+        val result = IsochroneRouter.route(
+            startLat = 0.0,
+            startLon = 0.0,
+            destLat = destLat,
+            destLon = destLon,
+            startTimeMs = startTimeMs,
+            stepMs = oneHourMs,
+            polars = BoatPolars.DEFAULT,
+            windAt = constantWind,
+            arrivalRadiusM = 2_000.0  // 2 km arrival radius
+        )
+        assertNotNull("Should find a route", result)
+        result!!
+        assertTrue("Path should have at least 2 points (start + arrival)", result.path.size >= 2)
+        assertEquals("Path should start at origin", 0.0, result.path.first().lat, 1e-6)
+        assertEquals("ETA should be after start", startTimeMs, result.etaMs - oneHourMs)
+    }
+
+    @Test
+    fun `route returns null when polars produce zero speed`() {
+        val zeroPolar = BoatPolars(emptyMap())
+        val result = IsochroneRouter.route(
+            startLat = 0.0,
+            startLon = 0.0,
+            destLat = 1.0,
+            destLon = 0.0,
+            startTimeMs = startTimeMs,
+            stepMs = oneHourMs,
+            polars = zeroPolar,
+            windAt = { _, _, _ -> WindForecast(0.0, 0.0, startTimeMs, 10.0, 0.0) },
+            maxSteps = 3
+        )
+        assertNull("Should return null when no progress is possible", result)
+    }
+
+    @Test
+    fun `backtrace returns path from start to arrival in order`() {
+        val p0 = RoutePoint(0.0, 0.0, 0L)
+        val p1 = RoutePoint(1.0, 0.0, 1L, parent = p0)
+        val p2 = RoutePoint(2.0, 0.0, 2L, parent = p1)
+        val path = IsochroneRouter.backtrace(p2)
+        assertEquals(3, path.size)
+        assertEquals(p0, path[0])
+        assertEquals(p1, path[1])
+        assertEquals(p2, path[2])
+    }
+}