package ballistics.flightLine

import ballistics.types.EarthType
import ballistics.types.OrbitalPoint
import ballistics.types.Orientation
import ballistics.types.THBLPoint
import ballistics.types.WorkCSType
import ballistics.utils.astro.AstronomerJ2000
import ballistics.utils.math.Matrix3D
import ballistics.utils.math.Vector3D
import kotlin.math.asin
import kotlin.math.cos
import kotlin.math.pow
import kotlin.math.sin
import kotlin.math.sqrt

class PointOnEarthCalculator(val earthType: EarthType, private val wcs: WorkCSType) {
    private var astro = AstronomerJ2000(earthType)



    fun pointOnEarth(
        point: OrbitalPoint,
        orientation: Orientation,
        x : Double,
        y : Double,
        focus : Double

    ): THBLPoint? {
        val krenMax = asin((astro.earth.polarRadius - 50000.0) / point.r.module())
        if (orientation.kren > krenMax) {
            orientation.kren = krenMax
        }
        if (orientation.kren < -krenMax) {
            orientation.kren = -krenMax
        }

        val rabs: Vector3D
        val vabs: Vector3D

        if (wcs == WorkCSType.WCSPath) {
            rabs = point.r
            vabs = point.v
        } else {
            val ask = astro.grinvToASK(point)

            rabs = ask.r
            vabs = ask.v
        }
        val dd: Vector3D
        val c = Matrix3D()
        c.second = rabs.basis()
        c.third = (vabs.rem(rabs)).basis()
        c.first = c.second.rem(c.third)
        var ct = c.transpose()
        val aa =
            Matrix3D(
                Vector3D(
                    cos(orientation.tang) * cos(orientation.risk) - sin(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
                    -sin(orientation.tang) * cos(orientation.kren),
                    cos(orientation.tang) * sin(orientation.risk) + sin(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
                ),
                Vector3D(
                    sin(orientation.tang) * cos(orientation.risk) + cos(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
                    cos(orientation.tang) * cos(orientation.kren),
                    sin(orientation.tang) * sin(orientation.risk) - cos(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
                ),
                Vector3D(
                    -cos(orientation.kren) * sin(orientation.risk),
                    sin(orientation.kren),
                    cos(orientation.kren) * cos(orientation.risk),
                ),
            )
        if (wcs == WorkCSType.WCSOrbit) {
            val g = Matrix3D()
            g.makeOzMatrix(astro.si2000(point.t))
            ct = g.transpose() * ct
        }

        val k = ct * aa

        val d = Vector3D(y / 1000.0, focus / 1000.0, x / 1000).basis()

        dd = k * d

        return earthIntersection(point.t, point.r, dd)
    }


    fun pointOnEarth(
        point: OrbitalPoint,
        orientation: Orientation,
    ): THBLPoint? {
        val krenMax = asin((astro.earth.polarRadius - 50000.0) / point.r.module())
        if (orientation.kren > krenMax) {
            orientation.kren = krenMax
        }
        if (orientation.kren < -krenMax) {
            orientation.kren = -krenMax
        }

        val rabs: Vector3D
        val vabs: Vector3D

        if (wcs == WorkCSType.WCSPath) {
            rabs = point.r
            vabs = point.v
        } else {
            val ask = astro.grinvToASK(point)

            rabs = ask.r
            vabs = ask.v
        }
        val dd: Vector3D
        val c = Matrix3D()
        c.second = rabs.basis()
        c.third = (vabs.rem(rabs)).basis()
        c.first = c.second.rem(c.third)
        var ct = c.transpose()
        val aa =
            Matrix3D(
                Vector3D(
                    cos(orientation.tang) * cos(orientation.risk) - sin(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
                    -sin(orientation.tang) * cos(orientation.kren),
                    cos(orientation.tang) * sin(orientation.risk) + sin(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
                ),
                Vector3D(
                    sin(orientation.tang) * cos(orientation.risk) + cos(orientation.tang) * sin(orientation.kren) * sin(orientation.risk),
                    cos(orientation.tang) * cos(orientation.kren),
                    sin(orientation.tang) * sin(orientation.risk) - cos(orientation.tang) * sin(orientation.kren) * cos(orientation.risk),
                ),
                Vector3D(
                    -cos(orientation.kren) * sin(orientation.risk),
                    sin(orientation.kren),
                    cos(orientation.kren) * cos(orientation.risk),
                ),
            )
        if (wcs == WorkCSType.WCSOrbit) {
            val g = Matrix3D()
            g.makeOzMatrix(astro.si2000(point.t))
            ct = g.transpose() * ct
        }

        val k = ct * aa

        val d = Vector3D(0.0, 1.0, 0.0).basis()

        dd = k * d

        return earthIntersection(point.t, point.r, dd)
    }

    /**
     * расчет точки пересечени отрезка, определенного вектором dd из точка rotn и земной поверхности
     */
    private fun earthIntersection(
        t: Double,
        rotn: Vector3D,
        dd: Vector3D,
    ): THBLPoint? {
        var dz = dd.z
        if (dz < 0) dz *= -1
        val x1: Double
        val y1: Double
        val z1: Double
        val x2: Double
        val y2: Double
        val z2: Double
        val r: Vector3D
        val h: Double

        val ekvRadiusOverPolarRadiusSquared = (astro.earth.ekvRadius / astro.earth.polarRadius).pow(2)
        if (dz < 0.1) {
            val k1 = dd.y / dd.x
            val k2 = rotn.y - k1 * rotn.x
            val k3 = dd.z / dd.x
            val k4 = rotn.z - k3 * rotn.x
            val k5 = k1 * k1 + ekvRadiusOverPolarRadiusSquared * k3 * k3 + 1
            val k6 = 2 * (k1 * k2 + ekvRadiusOverPolarRadiusSquared * k3 * k4)
            val k7 = k2 * k2 + ekvRadiusOverPolarRadiusSquared * k4 * k4 - astro.earth.ekvRadius * astro.earth.ekvRadius

            if ((k6 * k6 - k5 * k7 * 4) < 0) {
                return null
            }


            x1 = (-k6 + sqrt(k6 * k6 - 4 * k5 * k7)) / (2 * k5)
            y1 = k1 * x1 + k2
            z1 = k3 * x1 + k4


            x2 = (-k6 - sqrt(k6 * k6 - 4 * k5 * k7)) / (2 * k5)
            y2 = k1 * x2 + k2
            z2 = k3 * x2 + k4
        } else {
            val k1 = dd.x / dd.z
            val k2 = rotn.x - k1 * rotn.z
            val k3 = dd.y / dd.z
            val k4 = rotn.y - k3 * rotn.z
            val k5 = k1 * k1 + k3 * k3 + ekvRadiusOverPolarRadiusSquared
            val k6 = 2 * (k1 * k2 + k3 * k4)
            val k7 = k2 * k2 + k4 * k4 - astro.earth.ekvRadius * astro.earth.ekvRadius

            if ((k6 * k6 - 4 * k5 * k7) < 0) {
                return null
            }


            z1 = (-k6 + sqrt(k6 * k6 - 4 * k5 * k7)) / (2 * k5)
            y1 = k3 * z1 + k4
            x1 = k1 * z1 + k2


            z2 = (-k6 - sqrt(k6 * k6 - 4 * k5 * k7)) / (2 * k5)
            y2 = k3 * z2 + k4
            x2 = k1 * z2 + k2
        }
        val d1 = Vector3D(rotn.x - x1, rotn.y - y1, rotn.z - z1).module()
        val d2 = Vector3D(rotn.x - x2, rotn.y - y2, rotn.z - z2).module()


        if (d1 <= d2) {
            r = Vector3D(x1, y1, z1)
            h = d1
        } else {
            r = Vector3D(x2, y2, z2)
            h = d2
        }

        val blh = astro.earth.xyz2blh(r)

        return THBLPoint(blh.lat, blh.long, h, astro.sunAngle(t, r))
    }

    /**
     *  скорость компенсации (чтобы получить СДИ надо домножить на фокусное расстояние в мм)
     */
    fun calculateWD(
        point: OrbitalPoint,
        orientation: Orientation,
        range: Double,
    ): Double {
        try {
            val r = point.r.module()

            val c1: Double = point.r.y * point.v.z - point.r.z * point.v.y
            val c2: Double = point.r.z * point.v.x - point.r.x * point.v.z
            val c3: Double = point.r.x * point.v.y - point.r.y * point.v.x
            val cc = sqrt(c1 * c1 + c2 * c2 + c3 * c3)

            // Трансверсальная скорость V*sin(Q)
            val worb = cc / r

            val wd = worb * (r - range * cos(orientation.kren)) / r / range

            return wd
        } catch (ex: Exception) {
            return 0.0
        }
    }
}
