package ballistics.zrv

import ballistics.orbitalPoints.AbstractOrbPointsCalculator
import ballistics.types.*
import ballistics.utils.earth.getEarth
import ballistics.utils.math.Vector3D
import ballistics.utils.math.equations.EquationCalculator2Div
import ballistics.utils.math.minmax.MinMaxGoldenSectCalculator
import kotlin.math.PI
import kotlin.math.atan
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

/**
 * Класс расчета ЗРВ КА-ППИ итерационным методом
 * Осуществляется проход по точкам орбиты с заданным шагом и формирование участков
 * видимости КА-ППИ с уточнением моментов входа и выхода в/из зоны видимости
 *
 */
internal class ZRVStepperCalculator(opc: AbstractOrbPointsCalculator, ppi: List<PPI>) {
    var opc: AbstractOrbPointsCalculator = opc
    private var ppiExt = mutableListOf<PPIExt>()
    private var indPPI = 0
    private var step = 60.0
    private val eart = getEarth(opc.earthType)
    var ppi = ppi
    var zrv = mutableListOf<ZRV>()

    /**
     * Расчет радиус-вектора КА в пунктовой СК текущего ППИ
     * ППИ определяется по индексу _indPPI
     */
    private fun gskToPointSK(point: OrbitalPoint): Vector3D {
        val r1 =
            Vector3D(
                point.r.z - ppiExt[indPPI].params.zn,
                point.r.x - ppiExt[indPPI].params.xn,
                point.r.y - ppiExt[indPPI].params.yn,
            )

        val r4 = r1.y * ppiExt[indPPI].params.cl + r1.z * ppiExt[indPPI].params.sl
        return Vector3D(
            ppiExt[indPPI].params.cb * r1.x - ppiExt[indPPI].params.sb * r4,
            ppiExt[indPPI].params.sb * r1.x + ppiExt[indPPI].params.cb * r4,
            ppiExt[indPPI].params.cl * r1.z - ppiExt[indPPI].params.sl * r1.y,
        )
    }

    /**
     * Расчет параметров видимости КА-ППИ для заданного положения КА
     * ППИ определяется по индексу _indPPI
     */
    private fun calculateViewParams(point: OrbitalPoint) {
        ppiExt[indPPI].t2 = point.t
        val r1 = point.v.x * ppiExt[indPPI].params.cl + point.v.y * ppiExt[indPPI].params.sl
        ppiExt[indPPI].vy2 = point.v.z * ppiExt[indPPI].params.sb + r1 * ppiExt[indPPI].params.cb
        val r = gskToPointSK(point)
        ppiExt[indPPI].gam2 = atan2(r.y, sqrt(r.x * r.x + r.z * r.z))
        ppiExt[indPPI].kaY = r.y
        ppiExt[indPPI].a2 = atan2(r.z, r.x)
        if (ppiExt[indPPI].a2 < 0) {
            ppiExt[indPPI].a2 += 2 * PI
        }
    }

    /**
     * Расчет зон радио-видимости на зааднном интервале времени
     */
    fun calculate(
        tn: Double,
        tk: Double,
    ): BallisticsError {
        zrv.clear()
        if (ppi.isEmpty()) return BallisticsError.EMPTY_PPI_LIST

        var gam1 : Double
        var gam2 : Double

        step = 60.0

        val eqc = EquationCalculator2Div()
        val minmax = MinMaxGoldenSectCalculator()
        minmax.sign = -1
        minmax.delta = 0.0001

        eqc.delta = 0.001 * PI / 180.0
        eqc.maxIterations = 50
        ppiExt.clear()
        for (p in ppi) {
            ppiExt.add(
                PPIExt().apply {
                    params.nip = p.ppiNum
                    params.au = p.auNum
                    params.sb = sin(p.lat)
                    params.cb = cos(p.lat)
                    params.sl = sin(p.long)
                    params.cl = cos(p.long)
                    elevMax = p.elevMax
                    elevMin = p.elevMin
                    val r = eart.blh2xyz(p.lat, p.long, p.height)
                    params.xn = r.x
                    params.yn = r.y
                    params.zn = r.z
                    params.bn = p.lat
                    shadowMin = p.shadowMin
                    shadowMax = p.shadowMax
                    // !!!
                },
            )
        }

        val stepper = opc.getStepper()

        val cnt = ppiExt.count()
        var ct = tn

        // первая точка интервала расчета
        var point = stepper.calculate(tn)
        if (point == null) {
            return BallisticsError.STEPPER_ERROR
        }
        for (i in 0..cnt - 1) {
            indPPI = i
            calculateViewParams(point)
            if (ppiExt[i].gam2 > ppiExt[i].elevMin) {
                ppiExt[i].zoneIn.t = point.t
                ppiExt[i].vit = point.vit
            }
        }
        ct += step
        while (ct < tk) {
            point = stepper.calculate(ct)
            if (point == null) {
                return BallisticsError.STEPPER_ERROR
            }
            for (i in 0..cnt - 1) {
                indPPI = i
                ppiExt[i].gam1 = ppiExt[i].gam2
                ppiExt[i].t1 = ppiExt[i].t2
                ppiExt[i].vy1 = ppiExt[i].vy2
                ppiExt[i].a1 = ppiExt[i].a2

                calculateViewParams(point)

                gam1 = ppiExt[i].gam1
                gam2 = ppiExt[i].gam2
                // начало зоны
                if (((gam1 < ppiExt[i].elevMin) && (gam2 > ppiExt[i].elevMin)) ||
                    (gam1 > ppiExt[i].elevMin && gam2 > ppiExt[i].elevMin && ppiExt[i].vit == 0)
                ) {
                    eqc.value = ppiExt[i].elevMin
                    var t = eqc.calculate(ppiExt[i].t1, ppiExt[i].t2, this::elevationAngle)

                    if (t != null) {
                        var buf = calcZoneParams(t)
                        var it = 0
                        while (buf.elevation <= ppiExt[i].elevMin && it < 15) {
                            t += 0.005
                            buf = calcZoneParams(t)
                            ++it
                        }
                        ppiExt[i].zoneIn.t = t
                        ppiExt[i].vit = point.vit
                    } else {
                        println(
                            """
                            Ошибка поиска момента входа в ЗРВ. 
                            ${ppiExt[i].gam1 * 180 / PI} - ${ppiExt[i].elevMin * 180 / PI} - ${ppiExt[i].gam2 * 180 / PI}
                            """.trimIndent(),
                        )
                        ppiExt[i].zoneIn.t = ppiExt[i].t2
                    }
                }
                // / максимум!
                if ((ppiExt[i].vy1 * ppiExt[i].vy2 < 0) &&
                    (ppiExt[i].gam1 > ppiExt[i].elevMin) &&
                    (ppiExt[i].kaY > 0)
                ) {
                    val tmax = minmax.calculate(ppiExt[i].t1, ppiExt[i].t2, this::elevationAngle)
                    if (tmax == null) {
                        println("Ошибка при поиске максимума ЗРВ")
                        ppiExt[i].zoneMax = calcZoneParams((ppiExt[i].t1 + ppiExt[i].t2) / 2)
                    } else {
                        ppiExt[i].zoneMax = calcZoneParams(tmax)
                    }
                }
                // конец зоны
                if ((
                        (ppiExt[i].gam1 > ppiExt[i].elevMin) &&
                            (gam2 < ppiExt[i].elevMin && ppiExt[i].vit != 0)
                    ) ||
                    (gam1 < ppiExt[i].elevMin && gam2 < ppiExt[i].elevMin && ppiExt[i].vit != 0)
                ) {
                    eqc.value = ppiExt[i].elevMin
                    var t = eqc.calculate(ppiExt[i].t1, ppiExt[i].t2, this::elevationAngle)

                    if (t != null) {
                        var buf = calcZoneParams(t)
                        var it = 0
                        while (buf.elevation <= ppiExt[i].elevMin && it < 15) {
                            t -= 0.005
                            buf = calcZoneParams(t)
                            ++it
                        }
                        // ppiExt[i].zoneOut.t = if (buf.elevation<=ppiExt[i].elevMin) t!! else t!! - 0.1
                        ppiExt[i].zoneOut.t = t
                    } else {
                        println("Ошибка поиска момента выхода в ЗРВ.")
                        ppiExt[i].zoneOut.t = ppiExt[i].t1
                    }
                    if (ppiExt[i].zoneOut.t * ppiExt[i].zoneIn.t > 1) {
                        addZone(i)
                    } else {

                        println(
                            "Ошибка расчета граничной точки зоны ${ppiExt[i].params.nip}    ${ppiExt[i].zoneIn.t}   ${
                                ppiExt[i].zoneOut.t
                            }   ",
                        )
                    }
                    ppiExt[i].vit = 0
                }
            }
            ct += step
        }
        return BallisticsError.OK
    }

    private fun addZone(ind: Int) {
        // разбиение зоны при наличии затенений
        if (ppiExt[ind].elevMax < ppiExt[ind].zoneMax.elevation ||
            (ppiExt[ind].shadowMin != null && ppiExt[ind].shadowMin!!.toList().isNotEmpty()) ||
            (ppiExt[ind].shadowMax != null && ppiExt[ind].shadowMax!!.toList().isNotEmpty())
        ) {
            var tn: Double? = null
            var inZone = false
            var inZoneNext = false
            var vzp = calcZoneParams(ppiExt[ind].zoneIn.t)

            if (ppiExt[ind].isVisible(vzp.azimuth, vzp.elevation)) {
                tn = ppiExt[ind].zoneIn.t
                inZone = true
            }
            val step = 0.1
            var t = ppiExt[ind].zoneIn.t + step
            while (t <= ppiExt[ind].zoneOut.t) {
                vzp = calcZoneParams(t)
                inZoneNext = ppiExt[ind].isVisible(vzp.azimuth, vzp.elevation)

                // конец видимости
                if (inZone && !inZoneNext) {
                    val z =
                        ZRV(
                            ppiExt[ind].params.nip,
                            ppiExt[ind].params.au,
                            ppiExt[ind].vit,
                            calcZoneParams(tn!!),
                            VisibilityParametersZRV(
                                ppiExt[ind].zoneMax.t,
                                ppiExt[ind].zoneMax.range,
                                ppiExt[ind].zoneMax.azimuth,
                                ppiExt[ind].zoneMax.elevation,
                            ),
                            calcZoneParams(t - step),
                        )
                    zrv.add(z)
                }
                // начало видимости
                if (!inZone && inZoneNext) {
                    tn = t
                }

                inZone = inZoneNext
                t += step
            }
            if (inZone && inZoneNext) {
                val z =
                    ZRV(
                        ppiExt[ind].params.nip,
                        ppiExt[ind].params.au,
                        ppiExt[ind].vit,
                        calcZoneParams(tn!!),
                        VisibilityParametersZRV(
                            ppiExt[ind].zoneMax.t,
                            ppiExt[ind].zoneMax.range,
                            ppiExt[ind].zoneMax.azimuth,
                            ppiExt[ind].zoneMax.elevation,
                        ),
                        calcZoneParams(ppiExt[ind].zoneOut.t),
                    )
                zrv.add(z)
            }
        } else {
            val z =
                ZRV(
                    ppiExt[ind].params.nip,
                    ppiExt[ind].params.au,
                    ppiExt[ind].vit,
                    calcZoneParams(ppiExt[ind].zoneIn.t),
                    calcZoneParams(ppiExt[ind].zoneMax.t),
                    calcZoneParams(ppiExt[ind].zoneOut.t),
                )
//            z.zoneIn.elevation = ppiExt[ind].elevMin
//            z.zoneOut.elevation = ppiExt[ind].elevMin
            zrv.add(z)
        }
    }

    fun clear(){
        opc.clear()
        zrv.clear()
    }

    private fun calcZoneParams(t: Double): VisibilityParametersZRV {
        val z = VisibilityParametersZRV()

        val stepper = opc.getStepper()
        val point: OrbitalPoint? = stepper.calculate(t) ?: return z

        val r = gskToPointSK(point!!)

        z.t = t
        z.elevation = atan(r.y / sqrt(r.x * r.x + r.z * r.z))
        z.range = r.module()
        z.azimuth = atan2(r.z, r.x)
        if (z.azimuth < 0) {
            z.azimuth += 2 * PI
        }
        return z
    }

    private fun elevationAngle(t: Double): Double {
        val stepper = opc.getStepper()
        val point = stepper.calculate(t)
        val r = gskToPointSK(point!!)
        return atan(r.y / sqrt(r.z * r.z + r.x * r.x))
    }
}
