package com.example.androidapp.routing

import org.terst.nav.data.model.BoatPolars
import org.terst.nav.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))
    }
}