package org.terst.nav

import kotlin.math.abs
import kotlin.math.cos
import kotlin.math.max
import kotlin.math.min

// Represents a single point on a polar curve: True Wind Angle and target Boat Speed
data class PolarPoint(val tWa: Double, val bSp: Double)

// Represents a polar curve for a specific True Wind Speed
data class PolarCurve(val twS: Double, val points: List<PolarPoint>) {
    init {
        require(points.isNotEmpty()) { "PolarCurve must have at least one point." }
        require(points.all { it.tWa in 0.0..180.0 }) {
            "TWA in PolarCurve must be between 0 and 180 degrees."
        }
        require(points.zipWithNext().all { it.first.tWa < it.second.tWa }) {
            "PolarPoints in a PolarCurve must be sorted by TWA."
        }
    }

    /**
     * Interpolates the target boat speed for a given True Wind Angle (TWA)
     * within this specific polar curve (constant TWS).
     */
    fun interpolateBsp(twa: Double): Double {
        val absoluteTwa = abs(twa)
        if (absoluteTwa < points.first().tWa) return points.first().bSp
        if (absoluteTwa > points.last().tWa) return points.last().bSp

        for (i in 0 until points.size - 1) {
            val p1 = points[i]
            val p2 = points[i + 1]
            if (absoluteTwa >= p1.tWa && absoluteTwa <= p2.tWa) {
                val ratio = (absoluteTwa - p1.tWa) / (p2.tWa - p1.tWa)
                return p1.bSp + ratio * (p2.bSp - p1.bSp)
            }
        }
        return 0.0
    }

    /**
     * Calculates the Velocity Made Good (VMG) for a given TWA and BSP.
     * VMG = BSP * cos(TWA)
     */
    fun calculateVmg(twa: Double, bsp: Double): Double {
        return bsp * cos(Math.toRadians(twa))
    }

    /**
     * Finds the TWA that yields the maximum upwind VMG for this polar curve.
     */
    fun findOptimalUpwindTwa(): Double {
        var maxVmg = -Double.MAX_VALUE
        var optimalTwa = 0.0
        // Search through TWA 0 to 90
        for (twa in 0..90) {
            val bsp = interpolateBsp(twa.toDouble())
            val vmg = calculateVmg(twa.toDouble(), bsp)
            if (vmg > maxVmg) {
                maxVmg = vmg
                optimalTwa = twa.toDouble()
            }
        }
        return optimalTwa
    }

    /**
     * Finds the TWA that yields the maximum downwind VMG for this polar curve.
     */
    fun findOptimalDownwindTwa(): Double {
        var maxVmg = -Double.MAX_VALUE // We want the most negative VMG for downwind
        var optimalTwa = 180.0
        // Search through TWA 90 to 180
        // For downwind, VMG is negative (moving away from wind)
        // We look for the minimum value (largest absolute negative)
        for (twa in 90..180) {
            val bsp = interpolateBsp(twa.toDouble())
            val vmg = calculateVmg(twa.toDouble(), bsp)
            if (vmg < maxVmg) {
                maxVmg = vmg
                optimalTwa = twa.toDouble()
            }
        }
        return optimalTwa
    }
}

// Represents the complete polar table for a boat, containing multiple PolarCurves for different TWS
data class PolarTable(val curves: List<PolarCurve>) {
    init {
        require(curves.isNotEmpty()) { "PolarTable must have at least one curve." }
        require(curves.zipWithNext().all { it.first.twS < it.second.twS }) {
            "PolarCurves in a PolarTable must be sorted by TWS."
        }
    }

    /**
     * Interpolates the target boat speed for a given True Wind Speed (TWS) and True Wind Angle (TWA).
     */
    fun interpolateBsp(tws: Double, twa: Double): Double {
        if (tws <= curves.first().twS) return curves.first().interpolateBsp(twa)
        if (tws >= curves.last().twS) return curves.last().interpolateBsp(twa)

        for (i in 0 until curves.size - 1) {
            val c1 = curves[i]
            val c2 = curves[i + 1]
            if (tws >= c1.twS && tws <= c2.twS) {
                val ratio = (tws - c1.twS) / (c2.twS - c1.twS)
                val bsp1 = c1.interpolateBsp(twa)
                val bsp2 = c2.interpolateBsp(twa)
                return bsp1 + ratio * (bsp2 - bsp1)
            }
        }
        return 0.0
    }

    /**
     * Finds the optimal upwind TWA for a given TWS by interpolating between curves.
     */
    fun findOptimalUpwindTwa(tws: Double): Double {
        if (tws <= curves.first().twS) return curves.first().findOptimalUpwindTwa()
        if (tws >= curves.last().twS) return curves.last().findOptimalUpwindTwa()

        for (i in 0 until curves.size - 1) {
            val c1 = curves[i]
            val c2 = curves[i + 1]
            if (tws >= c1.twS && tws <= c2.twS) {
                val ratio = (tws - c1.twS) / (c2.twS - c1.twS)
                return c1.findOptimalUpwindTwa() + ratio * (c2.findOptimalUpwindTwa() - c1.findOptimalUpwindTwa())
            }
        }
        return 0.0
    }

    /**
     * Finds the optimal downwind TWA for a given TWS by interpolating between curves.
     */
    fun findOptimalDownwindTwa(tws: Double): Double {
        if (tws <= curves.first().twS) return curves.first().findOptimalDownwindTwa()
        if (tws >= curves.last().twS) return curves.last().findOptimalDownwindTwa()

        for (i in 0 until curves.size - 1) {
            val c1 = curves[i]
            val c2 = curves[i + 1]
            if (tws >= c1.twS && tws <= c2.twS) {
                val ratio = (tws - c1.twS) / (c2.twS - c1.twS)
                return c1.findOptimalDownwindTwa() + ratio * (c2.findOptimalDownwindTwa() - c1.findOptimalDownwindTwa())
            }
        }
        return 0.0
    }

    /**
     * Calculates the "Polar Percentage" for current boat performance.
     * Polar % = (Actual BSP / Target BSP) * 100
     * @return Polar percentage, or 0.0 if target BSP cannot be determined.
     */
    fun calculatePolarPercentage(currentTwS: Double, currentTwa: Double, currentBsp: Double): Double {
        val targetBsp = interpolateBsp(currentTwS, currentTwa)
        return if (targetBsp > 0) {
            (currentBsp / targetBsp) * 100.0
        } else {
            0.0
        }
    }
}