package ballistics.orbitalPoints.timeStepper

import ballistics.orbitalPoints.AbstractOrbPointsCalculator
import ballistics.orbitalPoints.integrator.AbstractIntegrator
import ballistics.orbitalPoints.integrator.IntegratorRK4
import ballistics.types.BallisticsError
import ballistics.types.EarthType
import ballistics.types.ModDVType
import ballistics.types.OrbitalPoint
import ballistics.utils.math.Vector3D
import kotlin.math.abs
import kotlin.math.round

class RungeStepper(var mdType: ModDVType, earthType: EarthType) : AbstractStepper {

    private val integrator: AbstractIntegrator = IntegratorRK4(mdType, earthType) // ModDVType.FOTO//BARS
    var points = mutableListOf<OrbitalPoint>()

    constructor(source: AbstractOrbPointsCalculator) : this(ModDVType.FOTO, source.earthType) {
        points = source.points
    }

    constructor(points: MutableList<OrbitalPoint>, earth: EarthType) : this(ModDVType.FOTO, earth) {
        this.points = points
    }

    private fun findPoint(t: Double): OrbitalPoint? {
        val cnt = points.size
        val dt = t - points.first().t
        var ind: Int = round(dt / integrator.step).toInt()
        if (ind >= -1 && ind < cnt + 1) {
            if (ind < 0) {
                ind = 0
            }
            if (ind >= cnt) {
                ind = cnt - 1
            }
            while ((t - points[ind].t > integrator.step) && (ind < points.size)) {
                ++ind
            }
            if (t - points[ind].t < 0 && ind > 0) {
                --ind
            }
            return points[ind]
        }
        return null
    }

    override fun calculate(t: Double): OrbitalPoint? {
        val p = findPoint(t)
        return p?.let {
            val dt = t - p.t
            val st = integrator.step
            integrator.step = dt

            val x = arrayOf(p.v.x, p.v.y, p.v.z, p.r.x, p.r.y, p.r.z, p.t)
            val r = integrator.nextStep(x)
            if (r != BallisticsError.OK) {
                return null
            }

            var vit = p.vit
            if (dt >= 0 && p.r.z > 0 && x[5] < 0 && p.v.z > 0) {
                vit++
            }
            if (dt < 0 && p.r.z < 0 && x[5] > 0 && p.v.z > 0) {
                vit--
            }

            integrator.step = st
            OrbitalPoint(
                integrator.y[6],
                vit,
                Vector3D(integrator.y[3], integrator.y[4], integrator.y[5]),
                Vector3D(integrator.y[0], integrator.y[1], integrator.y[2]),
            )
        } ?: run {
            null
        }
    }

    override fun clear() {
        points.clear()
    }

    override fun calculate(
        t: Double,
        p: OrbitalPoint,
    ): OrbitalPoint? {
        val dt = t - p.t
        if (abs(dt) > 80.0) {
            return null
        }

        val st = integrator.step
        integrator.step = dt

        val x = arrayOf(p.v.x, p.v.y, p.v.z, p.r.x, p.r.y, p.r.z, p.t)
        if (integrator.nextStep(x) != BallisticsError.OK) {
            return null
        }

        var vit = p.vit
        if (dt >= 0 && p.r.z > 0 && x[5] < 0 && p.v.z > 0) {
            vit++
        }
        if (dt < 0 && p.r.z < 0 && x[5] > 0 && p.v.z > 0) {
            vit--
        }

        integrator.step = st
        return OrbitalPoint(
            integrator.y[6],
            vit,
            Vector3D(integrator.y[3], integrator.y[4], integrator.y[5]),
            Vector3D(integrator.y[0], integrator.y[1], integrator.y[2]),
        )
    }
}
