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.Vector
import org.nstart.dep265.tletools.zeptomoby.core.eci.EciTime
import org.nstart.dep265.tletools.zeptomoby.core.exceptions.DecayException
import org.nstart.dep265.tletools.zeptomoby.core.exceptions.PropagationException
import org.nstart.dep265.tletools.zeptomoby.orbit.Orbit
import kotlin.math.*

abstract class NoradBase(o: Orbit) {

    abstract fun position(tsince: Double): EciTime
    abstract fun clone(o: Orbit): NoradBase

    protected fun finalPosition(incl: Double, omega: Double, e: Double, a: Double,
                                xl: Double, xnode: Double, xn: Double, tsince: Double): EciTime {
        if ((e * e) > 1.0) throw PropagationException("Error in satellite data")

        val beta = sqrt(1.0 - e * e)
        val axn  = e * cos(omega)
        var temp = 1.0 / (a * beta * beta)

        val sinip = sin(orbit.inclination)
        val cosip = cos(orbit.inclination)
        val aycof = 0.25 * a3ovk2 * sinip
        val xlcof = (0.125 * a3ovk2 * sinip * (3.0 + 5.0 * cosip)) /
                (1.0 + cosip)
        val xll  = temp * xlcof * axn
        val aynl = temp * aycof
        val xlt  = xl + xll
        val ayn  = e * sin(omega) + aynl

        val e6a = 1.0e-6
        val capu   = Globals.fmod2p(xlt - xnode)

        var temp2  = capu
        var temp3  = 0.0
        var temp4  = 0.0
        var temp5  = 0.0
        var temp6  = 0.0
        var sinepw = 0.0
        var cosepw = 0.0
        var fDone  = false

        for (i in 1..10) {
            if (fDone) break

            sinepw = sin(temp2)
            cosepw = cos(temp2)
            temp3 = axn * sinepw
            temp4 = ayn * cosepw
            temp5 = axn * cosepw
            temp6 = ayn * sinepw

            val epw = (capu - temp4 + temp3 - temp2) / (1.0 - temp5 - temp6) + temp2

            if (abs(epw - temp2) <= e6a) fDone = true
            else temp2 = epw
        }

        val ecose = temp5 + temp6
        val esine = temp3 - temp4
        val elsq  = axn * axn + ayn * ayn
        temp  = 1.0 - elsq
        val pl = a * temp
        val r  = a * (1.0 - ecose)
        var temp1 = 1.0 / r
        val rdot  = Globals.xke * sqrt(a) * esine * temp1
        val rfdot = Globals.xke * sqrt(pl) * temp1
        temp2 = a * temp1
        val betal = sqrt(temp)
        temp3 = 1.0 / (1.0 + betal)
        val cosu = temp2 * (cosepw - axn + ayn * esine * temp3)
        val sinu = temp2 * (sinepw - ayn - axn * esine * temp3)
        val u = Globals.acTan(sinu, cosu)
        val sin2u = 2.0 * sinu * cosu
        val cos2u = 2.0 * cosu * cosu - 1.0

        temp  = 1.0 / pl
        temp1 = Globals.ck2 * temp
        temp2 = temp1 * temp

        val cosip2 = cosip * cosip
        val x3thm1 = 3.0 * cosip2 - 1.0
        val x1mth2 = 1.0 - cosip2
        val x7thm1 = 7.0 * cosip2 - 1.0
        val rk = r * (1.0 - 1.5 * temp2 * betal * x3thm1) +
                0.5 * temp1 * x1mth2 * cos2u
        val uk = u - 0.25 * temp2 * x7thm1 * sin2u
        val xnodek = xnode + 1.5 * temp2 * cosio * sin2u
        val xinck = incl + 1.5 * temp2 * cosio * sinio * cos2u
        val rdotk = rdot - xn * temp1 * x1mth2 * sin2u
        val rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1)

        val sinuk = sin(uk)
        val cosuk = cos(uk)
        val sinik = sin(xinck)
        val cosik = cos(xinck)
        val sinnok = sin(xnodek)
        val cosnok = cos(xnodek)
        val xmx = -sinnok * cosik
        val xmy = cosnok * cosik
        val ux  = xmx * sinuk + cosnok * cosuk
        val uy  = xmy * sinuk + sinnok * cosuk
        val uz  = sinik * sinuk
        val vx  = xmx * cosuk - cosnok * sinuk
        val vy  = xmy * cosuk - sinnok * sinuk
        val vz  = sinik * cosuk

        val x = rk * ux
        val y = rk * uy
        val z = rk * uz

        val vecPos = Vector(x, y, z)
        val altKm = (vecPos.magnitude() * (Globals.wgs.xkmPer / Globals.ae))

        if (altKm < Globals.wgs.xkmPer) {
            val decayTime = orbit.epoch

            decayTime.addMin(tsince)
            throw DecayException(decayTime, "${orbit.satName}#${orbit.satId}", "altitude < xkmper")
        }

        val xdot = rdotk * ux + rfdotk * vx
        val ydot = rdotk * uy + rfdotk * vy
        val zdot = rdotk * uz + rfdotk * vz

        val vecVel = Vector(xdot, ydot, zdot)

        val gmt = orbit.epoch
        gmt.addMin(tsince)

        return EciTime(vecPos, vecVel, gmt)
    }

    val orbit: Orbit = o

    val cosio: Double = cos(orbit.inclination); val sinio: Double = sin(orbit.inclination)
    val betao2: Double; val betao: Double
    var s4: Double
        protected set
    var qoms24: Double
        protected set
    val tsi: Double; val eta: Double
    val eeta: Double; val coef: Double; val coef1: Double
    val c1: Double; val c3: Double; val c4: Double
    val a3ovk2: Double; val xmdot: Double; val omgdot: Double
    val xnodot: Double; val xnodcf: Double; val t2cof: Double

    init {
        val theta2 = cosio * cosio
        val x3thm1 = 3.0 * theta2 - 1.0
        val eosq = sqr(orbit.eccentricity)
        betao2 = 1.0 - eosq
        betao  = sqrt(betao2)
        val rp = orbit.semiMajor * (1.0 - orbit.eccentricity)
        val perigee = (rp - 1.0) * Globals.wgs.xkmPer
        s4      = Globals.s
        qoms24  = Globals.qoms2t
        if (perigee < 156.0) {
            s4 = perigee - 78.0
            if (perigee <= 98.0) s4 = 20.0

            qoms24 = ((120.0 - s4) * Globals.ae / Globals.wgs.xkmPer).pow(4)
            s4 = s4 / Globals.wgs.xkmPer + Globals.ae
        }
        val pinvsq = 1.0 / (sqr(orbit.semiMajor) * sqr(betao2))
        tsi = 1.0 / (orbit.semiMajor - s4)
        eta = orbit.semiMajor * orbit.eccentricity * tsi
        eeta = orbit.eccentricity * eta
        val etasq = eta * eta
        val psisq = abs(1.0 - etasq)
        coef  = qoms24 * tsi.pow(4.0)
        coef1 = coef / psisq.pow(3.5)
        val c2 = coef1 * orbit.meanMotion *
                (orbit.semiMajor * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) +
                        0.75 * Globals.ck2 * tsi / psisq * x3thm1 *
                        (8.0 + 3.0 * etasq * (8.0 + etasq)))
        c1 = orbit.bStar * c2
        a3ovk2 = -Globals.xj3 / Globals.ck2 * Globals.ae.pow(3.0)
        c3 = coef * tsi * a3ovk2 * orbit.meanMotion * Globals.ae * sinio / orbit.eccentricity
        val x1mth2 = 1.0 - theta2
        c4 = 2.0 * orbit.meanMotion * coef1 * orbit.semiMajor * betao2 *
                (eta * (2.0 + 0.5 * etasq) +
                        orbit.eccentricity * (0.5 + 2.0 * etasq) -
                        2.0 * Globals.ck2 * tsi / (orbit.semiMajor * psisq) *
                        (-3.0 * x3thm1 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta)) +
                                0.75 * x1mth2 *
                                (2.0 * etasq - eeta * (1.0 + etasq)) *
                                cos(2.0 * orbit.argPerigee)))
        val theta4 = theta2 * theta2
        val temp1  = 3.0 * Globals.ck2 * pinvsq * orbit.meanMotion
        val temp2  = temp1 * Globals.ck2 * pinvsq
        val temp3  = 1.25 * Globals.ck4 * pinvsq * pinvsq * orbit.meanMotion
        xmdot = orbit.meanMotion + 0.5 * temp1 * betao * x3thm1 +
                0.0625 * temp2 * betao *
                (13.0 - 78.0 * theta2 + 137.0 * theta4)
        val x1m5th = 1.0 - 5.0 * theta2
        omgdot = -0.5 * temp1 * x1m5th + 0.0625 * temp2 *
                (7.0 - 114.0 * theta2 +  395.0 * theta4) +
                temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4)
        val xhdot1 = -temp1 * cosio
        xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) +
                2.0 * temp3 * (3.0 - 7.0 * theta2)) * cosio
        xnodcf = 3.5 * betao2 * xhdot1 * c1
        t2cof  = 1.5 * c1
    }
}