package ballistics.orbitalPoints

import ballistics.orbitalPoints.integrator.AbstractIntegrator
import ballistics.orbitalPoints.integrator.IntegratorAdams
import ballistics.orbitalPoints.integrator.IntegratorRK4
import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.orbitalPoints.timeStepper.RungeStepper
import ballistics.types.BallisticsError
import ballistics.types.EarthType
import ballistics.types.InitialConditions
import ballistics.types.IntegrationType
import ballistics.types.ModDVType
import ballistics.types.OrbitalPoint
import ballistics.utils.math.Vector3D
import ballistics.utils.math.equations.AbstractEquationCalculator
import ballistics.utils.math.equations.EquationCalculatorSpan
import kotlin.math.abs

internal class OrbitalPointsIntegrator(val moddv: ModDVType, integratorType: IntegrationType, earthType: EarthType) :
    AbstractOrbPointsCalculator(earthType) {
    private val integrator: AbstractIntegrator
    private val fastStepper: IntegratorRK4
    var mic = mutableListOf<InitialConditions>()
    private var x = arrayOf<Double>()
    private var vit = 0
    val equationCalculator: AbstractEquationCalculator = EquationCalculatorSpan()
    var mTn : Double = 0.0

    init {
        when (integratorType) {
            IntegrationType.RUNG4 -> {
                integrator = IntegratorRK4(moddv, earthType)
                fastStepper = IntegratorRK4(moddv, earthType)
            }
            IntegrationType.ADAMS7 -> {
                integrator = IntegratorAdams(moddv, earthType)
                fastStepper = IntegratorRK4(moddv, earthType)
            }
        }
        equationCalculator.delta = 0.001
    }

    constructor(
        nu: InitialConditions,
        modDVType: ModDVType,
        integratorType: IntegrationType,
        earthType: EarthType,
    ) : this(modDVType, integratorType, earthType) {
        mic.clear()
        mic.add(nu)
    }
    constructor(
        nu: Array<InitialConditions>,
        modDVType: ModDVType,
        integratorType: IntegrationType,
        earthType: EarthType,
    ) : this(modDVType, integratorType, earthType) {
        mic.clear()
        mic.addAll(nu)
    }


    /**
     * Расчет точек орбиты
     * @param nu начальные условия движения центра масс КА
     * @param tn время начала расчета
     * @param tk время конца расчета
     */
    fun calculate(
        nu: InitialConditions,
        tn: Double,
        tk: Double,
    ): BallisticsError {
        mTn = tn
        var t = nu.point.t
        x = arrayOf(nu.point.v.x, nu.point.v.y, nu.point.v.z, nu.point.r.x, nu.point.r.y, nu.point.r.z, nu.point.t)

        integrator.setSBall(nu.sBall)
        fastStepper.setSBall(nu.sBall)
        integrator.setF81(nu.f81)
        fastStepper.setF81(nu.f81)
        integrator.accelerate(x)
        points.clear()
        revolutions.clear()

        val equationCalculator: AbstractEquationCalculator = EquationCalculatorSpan()
        equationCalculator.delta = 0.001

        vit = nu.point.vit


        if (abs(nu.point.r.z) < 0.00001 && nu.point.t >= tn) {
            addRevolution(nu.point.t)
        }

        while (t <= tk) {
            integrator.nextStep(x)
            nextPoint(t)
            for (i in 0..6)
                x[i] = integrator.y[i]
            t += integrator.step
        }

        return BallisticsError.OK
    }


    fun nextPoint(t : Double){
        if (x[5] < 0 && integrator.y[5] >= 0) {
            ++vit
            val tvuz = equationCalculator.calculate(t, t + integrator.step, this::equation)
            tvuz?.let {
                if (tvuz >= mTn) {
                    addRevolution(it)
                }
            }
        }
        if (t >= mTn) {
            val p = OrbitalPoint(t, vit, Vector3D(x[3], x[4], x[5]), Vector3D(x[0], x[1], x[2]))
            points.add(p)
        }
    }


    /**
     * Расчет точек орбиты
     * @param mIC массив начальных условий движения центра масс КА
     * @param tn время начала расчета
     * @param tk время конца расчета
     */
    fun calculate(
        mIC: Array<InitialConditions>,
        tn: Double,
        tk: Double,
    ): BallisticsError {
        points.clear()
        revolutions.clear()
        mTn = tn


        if (mIC.isEmpty())
            return BallisticsError.EMPTY_NU


        var t = mIC[0].point.t
        x =
            arrayOf(
                mIC[0].point.v.x,
                mIC[0].point.v.y,
                mIC[0].point.v.z,
                mIC[0].point.r.x,
                mIC[0].point.r.y,
                mIC[0].point.r.z,
                mIC[0].point.t,
            )
        integrator.setSBall(mIC[0].sBall)
        fastStepper.setSBall(mIC[0].sBall)
        integrator.setF81(mIC[0].f81)
        fastStepper.setF81(mIC[0].f81)
        integrator.accelerate(x)
        vit = mIC[0].point.vit

        var tkk: Double
        for (i in mIC.indices) {
            tkk = if (i == mIC.size - 1) tk else mIC[i + 1].point.t
            while (t < tkk) {
                integrator.nextStep(x)

                nextPoint(t)
                for (j in 0..6)
                    x[j] = integrator.y[j]
                t += integrator.step
            }

            if (i < mIC.size - 1) {
                if (points.isNotEmpty() && points.last().t >= mIC[i + 1].point.t) {
                    points.removeLast()
                }
                arrayOf(
                    mIC[i + 1].point.v.x,
                    mIC[i + 1].point.v.y,
                    mIC[i + 1].point.v.z,
                    mIC[i + 1].point.r.x,
                    mIC[i + 1].point.r.y,
                    mIC[i + 1].point.r.z,
                    mIC[i + 1].point.t,
                ).also { x = it }
                integrator.setSBall(mIC[i + 1].sBall)
                fastStepper.setSBall(mIC[i + 1].sBall)
                integrator.setF81(mIC[i + 1].f81)
                fastStepper.setF81(mIC[i + 1].f81)
                integrator.accelerate(x)

                vit = mIC[i + 1].point.vit
                t = mIC[i + 1].point.t
            }
        }
        return BallisticsError.OK
    }

    override fun calculate(
        tbegin: Double,
        tend: Double,
    ): BallisticsError {
        if (mic.isEmpty()) {
            return BallisticsError.EMPTY_NU
        } else if (mic.size == 1) {
            calculate(mic.first(), tbegin, tend)
        } else {
            calculate(mic.toTypedArray(), tbegin, tend)
        }
        return BallisticsError.OK
    }

    override fun getStepper(): AbstractStepper {
        return RungeStepper(this)
    }

    override fun fastStep(t: Double): OrbitalPoint {
        val dt = t - x[6]
        fastStepper.step = dt
        fastStepper.nextStep(x)
        val p : OrbitalPoint = OrbitalPoint(
                t,
                vit,
                Vector3D(fastStepper.y[3], fastStepper.y[4], fastStepper.y[5]),
                Vector3D(fastStepper.y[0], fastStepper.y[1], fastStepper.y[2]),
            )
        return p
    }

}
