package ballistics.utils.earth

import ballistics.types.BLHPoint
import ballistics.utils.math.Vector3D
import kotlin.math.PI
import kotlin.math.atan2
import kotlin.math.hypot
import kotlin.math.sqrt

open class EarthPZ90d0 : AbstractEarth() {
    init {
        fM = 398600.44e9
        ekvRadius = 6378136.0
        wEarth = 0.7292115e-4
        alphaEllips = 1 / 298.257839303
        middleRadius = 6378100.0
        polarRadius = (1 - alphaEllips) * ekvRadius
        c20 = -1.08262741e-3
        eElips2 = 2 * alphaEllips - alphaEllips * alphaEllips
    }

    override fun blh2xyz(
        b: Double,
        l: Double,
        h: Double,
    ): Vector3D {
        var v = Vector3D()
        val sb = Math.sin(b)
        val cb = Math.cos(b)
        val sl = Math.sin(l)
        val cl = Math.cos(l)
        val n = ekvRadius / Math.sqrt(1 - eElips2 * sb * sb)
        v.x = (n + h) * cb * cl
        v.y = (n + h) * cb * sl
        v.z = ((1 - eElips2) * n + h) * sb
        return v
    }

    override fun xyz2blh(r: Vector3D): BLHPoint {
        var l = atan2(r.y, r.x)
        if (l < 0.0) l = PI * 2.0 + l

        var r1: Double = hypot(r.x, r.y)
        val rr: Double = hypot(r1, r.z)
        val sinFiC: Double = r.z / rr
        val cosFiC = r1 / rr
        val fic: Double = atan2(r.z, r1)
        val ekvadrat: Double = (2.0 - alphaEllips) * alphaEllips
        r1 = 1.0 - ekvadrat * cosFiC * cosFiC
        val delta = ekvadrat * sinFiC * cosFiC / r1
        val rE: Double = ekvRadius * sqrt(1.0 - ekvadrat) / sqrt(r1)
        val eMal = rE * delta / rr
        var b = fic + eMal
        var h = ((rr - rE) * (1.0 - eMal * delta / 2.0))
        return BLHPoint(b, l, h)
    }
}
