package org.nstart.dep265.tletools.zeptomoby.orbit.norad

import org.nstart.dep265.tletools.zeptomoby.core.Globals
import org.nstart.dep265.tletools.zeptomoby.core.Globals.sqr
import org.nstart.dep265.tletools.zeptomoby.core.eci.EciTime
import org.nstart.dep265.tletools.zeptomoby.orbit.Orbit
import kotlin.math.cos
import kotlin.math.pow
import kotlin.math.sin

open class NoradSGP4(o: Orbit): NoradBase(o) {
    protected val c5: Double
    protected val omgcof: Double
    protected val xmcof: Double
    protected val delmo: Double
    protected val sinmo: Double

    init {
        val etasq: Double = eta * eta

        c5 = 2.0 * coef1 * orbit.semiMajor * betao2 * (1.0 + 2.75 * (etasq + eeta) + eeta * etasq)
        omgcof = orbit.bStar * c3 * cos(orbit.argPerigee)
        xmcof = -(2.0 / 3.0) * coef * orbit.bStar * Globals.ae / eeta
        delmo = (1.0 + eta * cos(orbit.meanAnomaly)).pow(3.0)
        sinmo = sin(orbit.meanAnomaly)
    }

    override fun position(tsince: Double): EciTime {
        var isImp = false

        if ((orbit.semiMajor * (1.0 - orbit.eccentricity) / Globals.ae) <
            (220.0 / Globals.wgs.xkmPer + Globals.ae)) isImp = true

        var d2 = 0.0
        var d3 = 0.0
        var d4 = 0.0

        var t3cof = 0.0
        var t4cof = 0.0
        var t5cof = 0.0

        if (!isImp) {
            val c1sq: Double = c1 * c1
            d2 = 4.0 * orbit.semiMajor * tsi * c1sq

            val temp: Double = d2 * tsi * c1 / 3.0
            d3 = (17.0 * orbit.semiMajor + s4) * temp
            d4 = 0.5 * temp * orbit.semiMajor * tsi *
                    (221.0 * orbit.semiMajor + 31.0 * s4) * c1
            t3cof = d2 + 2.0 * c1sq
            t4cof = 0.25 * (3.0 * d3 + c1 * (12.0 * d2 + 10.0 * c1sq))
            t5cof = 0.2 * (3.0 * d4 + 12.0 * c1 * d3 + 6.0 *
                    d2 * d2 + 15.0 * c1sq * (2.0 * d2 + c1sq))
        }

        val xmdf: Double = orbit.meanAnomaly + xmdot * tsince
        val omgadf: Double = orbit.argPerigee + omgdot * tsince
        val xnoddf: Double = orbit.raan + xnodot * tsince
        var omega = omgadf
        var xmp = xmdf
        val tsq = tsince * tsince
        val xnode: Double = xnoddf + xnodcf * tsq
        var tempa: Double = 1.0 - c1 * tsince
        var tempe: Double = orbit.bStar * c4 * tsince
        var templ: Double = t2cof * tsq

        if (!isImp) {
            val delomg: Double = omgcof * tsince
            val delm: Double = xmcof * ((1.0 + eta * cos(xmdf)).pow(3.0) - delmo)
            val temp = delomg + delm
            xmp = xmdf + temp
            omega = omgadf - temp
            val tcube = tsq * tsince
            val tfour = tsince * tcube
            tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour
            tempe += orbit.bStar * c5 * (sin(xmp) - sinmo)
            templ += t3cof * tcube + tfour * (t4cof + tsince * t5cof)
        }

        val a: Double = orbit.semiMajor * sqr(tempa)
        val e: Double = orbit.eccentricity - tempe
        val xl: Double = xmp + omega + xnode + orbit.meanMotion * templ
        val xn: Double = Globals.xke / a.pow(1.5)

        return finalPosition(orbit.inclination, omgadf, e, a, xl, xnode, xn, tsince)
    }

    override fun clone(o: Orbit): NoradBase = NoradSGP4(o)
}