package ballistics.mpl

import ballistics.types.EarthType
import ballistics.types.OrbitalPoint
import ballistics.types.Orientation
import ballistics.types.TangageType
import ballistics.types.WorkCSType
import ballistics.utils.astro.AstronomerJ2000
import ballistics.utils.math.Matrix3D
import ballistics.utils.math.Vector3D
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.atan
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sign
import kotlin.math.sin
import kotlin.math.sqrt

class OrientOnPointCalculator(val earthType: EarthType, val wcs: WorkCSType, val tangType: TangageType) {
    var astro = AstronomerJ2000(earthType)

    fun pointInWCS(
        ka: OrbitalPoint,
        point: Vector3D,
    ): Vector3D {
        var w: Vector3D
        if (wcs == WorkCSType.WCSOrbit) {

            val kaA = astro.grinvToASK(ka)

            val pa = astro.grinvToASK(point, ka.t)



            val c = Matrix3D()
            c.second = kaA.r.basis()
            c.third = (kaA.v.rem(kaA.r)).basis()
            c.first = c.second.rem(c.third)


            val d: Vector3D = pa - kaA.r

            w = c * d
        } else {
            val c = Matrix3D()
            c.second = ka.r.basis()
            c.third = (ka.v.rem(ka.r)).basis()
            c.first = c.second.rem(c.third)


            val d: Vector3D = point - ka.r

            w = c * d
        }

        return w
    }

    fun calculateOrientOnPoint(
        ka: OrbitalPoint,
        point: Vector3D,
    ): Orientation {
        val orient = Orientation(0.0, 0.0, 0.0)
        val w = pointInWCS(ka, point)

        if (tangType == TangageType.TTProactive) {
            orient.tang = PI - atan2(w.x, w.y)
            orient.kren = atan2(w.z, w.y / cos(orient.tang)) - PI
        } else {
            orient.kren = atan2(w.z, w.y) - PI
            orient.tang = PI - atan2(w.x, w.z / sin(orient.kren))
        }

        if (abs(orient.tang) > PI) {
            orient.tang -= sign(orient.tang) * PI * 2
        }
        if (abs(orient.kren) > PI) {
            orient.kren -= sign(orient.kren) * PI * 2
        }

        return orient
    }

    fun calculateVisirAngle(
        ka: OrbitalPoint,
        b: Double,
        l: Double,
        h: Double,
    ): Double {
        val rp = astro.earth.blh2xyz(b, l, h)
        val r1 = Vector3D()
        val r = Vector3D()

        r1.x = ka.r.z - rp.z
        r1.y = ka.r.x - rp.x
        r1.z = ka.r.y - rp.y

        val r4 = r1.y * cos(l) + r1.z * sin(l)

        r.x = cos(b) * r1.x - sin(b) * r4
        r.y = sin(b) * r1.x + cos(b) * r4
        r.z = cos(l) * r1.z - sin(l) * r1.y

        var angV: Double = sqrt(r.x * r.x + r.z * r.z)
        if (angV != 0.0) {
            angV = atan(r.y / sqrt(r.x * r.x + r.z * r.z))
            angV = PI / 2 - angV
        }

        return angV
    }
}
