package space.nstart.pcp.angularmotion

import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.types.EarthType
import ballistics.types.Orientation
import ballistics.utils.math.Matrix3D
import ballistics.utils.math.Quaternion3D
import ballistics.utils.math.Vector3D
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

/**
 * Режим азимутального сканирования, порт AzimuthAISTPUUD без AIST-префикса.
 *
 * В отличие от старого Qt-кода, расчет получает орбитальные точки через
 * ballistics.orbitalPoints.timeStepper.AbstractStepper из существующей ballistics-lib.
 */
open class AzimuthPUUD(
    stepper: AbstractStepper,
    earthType: EarthType = EarthType.PZ90d02,
    config: AngularMotionConfig = AngularMotionConfig(),
    private val sickle: Boolean = false,
) : AbstractPuudCalculator(stepper, earthType, config) {

    override fun calculate(id: SurveyId): AngularMotionResult {
        validate(id)
        if (id.sdi.isEmpty() || abs(id.sdi.first()) < 1.0e-4) {
            throw AngularMotionCalculationException("Некорректное значение СДИ")
        }

        val tn = calcTn(id)
        val points = calculateAzimuth(id, tn, id.duration)
        return AngularMotionResult(
            mode = AngularMotionMode.AZIMUTH,
            startTime = tn,
            points = points,
            tauPoints = calculateTauPoints(points),
            oeps = buildOeps(id, tn),
        )
    }

    protected open fun calculateAzimuth(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
        val step = calculationStep()
        val routeNormalGsk = routeNormalInGreenwich(id.b, id.l, id.h, id.azimuth)
        val routeDirectionGsk = routeDirectionInGreenwich(id.b, id.l, id.azimuth)
        val firstTargetGsk = astro.earth.blh2xyz(id.b, id.l, id.h)
        val initialSdi = sdiAt(id.sdi, 0.0)

        val firstPoint = pointAt(tn)
        val firstAbs = astro.grinvToASK(firstPoint)
        val targetAbs = astro.grinvToASK(firstTargetGsk, tn)
        val initialLiv = initialVisirQuaternion(
            rAbs = firstAbs.r,
            targetAbs = targetAbs,
            routeNormalGsk = routeNormalGsk,
            routeDirectionGsk = routeDirectionGsk,
            time = tn,
            reverse = initialSdi < 0.0,
        )
        var liv = alignVisirQuaternionToRoute(
            liv = initialLiv,
            time = tn,
            routeNormalGsk = routeNormalGsk,
            rAbs = firstAbs.r,
            reverse = initialSdi < 0.0,
        )

        val result = mutableListOf<AngularMotionPoint>()
        var t = tn
        var elapsed = 0.0
        var previous: AngularMotionPoint? = null

        while (elapsed <= duration + EPS) {
            val orbital = pointAt(t)
            val ask = astro.grinvToASK(orbital)
            val sdiForTime = sdiAt(id.sdi, elapsed)
            liv = alignVisirQuaternionToRoute(
                liv = liv,
                time = t,
                routeNormalGsk = routeNormalGsk,
                rAbs = ask.r,
                reverse = sdiForTime < 0.0,
            )
            val di = slantRangeFromQuaternion(liv, ask.r, t)
            val omegaVisir = ownCornerSpeed(t, liv, di, ask.r, ask.v, sdiForTime, routeNormalGsk)
            val orientation = orientationFromVisirQuaternion(orbital, liv)
            val point = buildIntegratedPoint(t, orientation, omegaVisir, previous, sickle)
            result += point
            previous = point

            if (elapsed + step <= duration + EPS) {
                liv = integrateQuaternionRK4(t, liv, step) { time, q ->
                    val p = pointAt(time)
                    val pAsk = astro.grinvToASK(p)
                    val pDi = slantRangeFromQuaternion(q, pAsk.r, time)
                    val pSdi = sdiAt(id.sdi, time - tn)
                    ownCornerSpeed(time, q, pDi, pAsk.r, pAsk.v, pSdi, routeNormalGsk)
                }
                val nextOrbital = pointAt(t + step)
                val nextAsk = astro.grinvToASK(nextOrbital)
                liv = alignVisirQuaternionToRoute(
                    liv = liv,
                    time = t + step,
                    routeNormalGsk = routeNormalGsk,
                    rAbs = nextAsk.r,
                    reverse = sdiAt(id.sdi, elapsed + step) < 0.0,
                )
            }
            elapsed += step
            t += step
        }

        return result
    }

    protected open fun ownCornerSpeed(
        time: Double,
        liv: Quaternion3D,
        di: Vector3D,
        rAbs: Vector3D,
        vAbs: Vector3D,
        sdi: Double,
        routeNormalGsk: Vector3D?,
    ): Vector3D {
        val omegaEarth = Vector3D(0.0, 0.0, astro.earth.wEarth)
        val relVelocityAbs = vAbs - omegaEarth.rem(rAbs)
        val relVelocityVisir = liv.inverse() * relVelocityAbs
        val range = di.module().coerceAtLeast(EPS)
        val vs = relVelocityVisir / range
        val omegaEarthVisir = liv.inverse() * omegaEarth
        return if (sickle) {
            Vector3D(0.0, vs.z + sdi / config.focus + omegaEarthVisir.z, -vs.y + omegaEarthVisir.y)
        } else {
            Vector3D(0.0, vs.z + omegaEarthVisir.y, -vs.y - sdi / config.focus + omegaEarthVisir.z)
        }
    }

    protected fun buildIntegratedPoint(
        t: Double,
        orientation: Orientation,
        omegaVisir: Vector3D,
        previous: AngularMotionPoint?,
        sickle: Boolean,
    ): AngularMotionPoint {
        val orbital = pointAt(t)
        val omega = visirToConnected(omegaVisir)
        val eps = previous?.let { if (abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } ?: Vector3D()
        val ground = requirePointOnEarth(orbital, orientation)
        val q = quaternionFor(orbital, orientation)
        val wd = wd(orbital, orientation, omega)
        val edgeSdi = edgeSdiValues(orbital, orientation, omega, sickle)
        val centerSdi = sdiForWd(wd, sickle)
        val sdiSpread = edgeSdi?.let { maxOf(abs(it.first - centerSdi), abs(it.second - centerSdi)) }
        return AngularMotionPoint(
            t = t,
            orbitalPoint = orbital,
            orientation = orientation,
            groundPoint = ground,
            omega = omega,
            eps = eps,
            quaternion = q,
            wd = wd,
            sdi = centerSdi,
            sdiLeft = edgeSdi?.first,
            sdiRight = edgeSdi?.second,
            sdiSpread = sdiSpread,
        )
    }

    protected fun visirToConnected(omegaVisir: Vector3D): Vector3D =
        conToVisirMatrix().transpose() * omegaVisir

    protected fun initialVisirQuaternion(
        rAbs: Vector3D,
        targetAbs: Vector3D,
        routeNormalGsk: Vector3D,
        routeDirectionGsk: Vector3D,
        time: Double,
        reverse: Boolean,
    ): Quaternion3D {
        val line = (targetAbs - rAbs).normSafe()
        val routeNormalAbs = astro.grinvToASK(routeNormalGsk, time).normSafe()
        val routeDirectionAbs = astro.grinvToASK(routeDirectionGsk, time).normSafe()

        // Азимут задаёт именно направление движения точки визирования по поверхности
        // Земли: 0° — на север, 90° — на восток. Поэтому ось сканирования визирной
        // СК строится по проекции азимутальной касательной на фокальную плоскость,
        // без смены знака. Иначе азимут 0° фактически превращается в 180°.
        var routeAxis = (routeDirectionAbs - line * (routeDirectionAbs * line)).normSafe()
        if (routeAxis.module() < EPS) {
            routeAxis = routeNormalAbs.rem(line).normSafe()
        }
        if (reverse) {
            routeAxis = routeAxis * -1.0
        }

        val crossAxis = line.rem(routeAxis).normSafe()
        val m = if (sickle) {
            rotationX(PI) * Matrix3D(
                line,
                crossAxis,
                line.rem(crossAxis).normSafe(),
            )
        } else {
            Matrix3D(line, routeAxis, crossAxis)
        }
        return quaternionFromMatrix(m).inverse().normalized()
    }

    /**
     * Доворачивает визирную систему вокруг текущей ЦЛВ так, чтобы ее ось
     * сканирования была направлена по заданному азимутальному маршруту на Земле.
     *
     * В старой реализации азимут участвовал только в начальной ориентации. После
     * интегрирования кватерниона оси фокальной плоскости могли дрейфовать вокруг
     * ЦЛВ, поэтому симметричные азимуты, например 20° и 340°, давали одинаковую
     * программу. Здесь азимутальный маршрут используется на каждом шаге: в текущей
     * точке визирования строится касательная к плоскости маршрута и визирная СК
     * поворачивается вокруг ЦЛВ до совпадения с этой касательной.
     */
    private fun alignVisirQuaternionToRoute(
        liv: Quaternion3D,
        time: Double,
        routeNormalGsk: Vector3D,
        rAbs: Vector3D,
        reverse: Boolean,
    ): Quaternion3D {
        val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
        if (lineAbs.module() < EPS) return liv

        val di = slantRangeFromQuaternion(liv, rAbs, time)
        if (di.module() < EPS) return liv

        val groundAbs = rAbs + di
        val groundGsk = astro.askToGrinvich(groundAbs, time)
        val groundBlh = astro.earth.xyz2blh(groundGsk)
        val normalGsk = ellipsoidNormalInGreenwich(groundBlh.lat, groundBlh.long)
        // Для плоскости маршрута с нормалью N_route и текущей нормали к ОЗЭ U
        // касательная в направлении заданного азимута равна U x N_route. Обратный
        // порядок N_route x U разворачивает маршрут: 0° ведёт на юг, 180° — на север.
        var desiredGsk = normalGsk.rem(routeNormalGsk).normSafe()
        if (desiredGsk.module() < EPS) return liv
        if (reverse) {
            desiredGsk = desiredGsk * -1.0
        }

        val desiredAbsRaw = astro.grinvToASK(desiredGsk, time).normSafe()
        val desiredAbs = (desiredAbsRaw - lineAbs * (desiredAbsRaw * lineAbs)).normSafe()
        if (desiredAbs.module() < EPS) return liv

        val currentAxisRaw = (liv * Vector3D(0.0, 1.0, 0.0)).normSafe()
        val currentAxis = (currentAxisRaw - lineAbs * (currentAxisRaw * lineAbs)).normSafe()
        if (currentAxis.module() < EPS) return liv

        val cosAngle = (currentAxis * desiredAbs).coerceIn(-1.0, 1.0)
        val sinAngle = lineAbs * currentAxis.rem(desiredAbs)
        val angle = atan2(sinAngle, cosAngle)
        if (abs(angle) < 1.0e-13) return liv

        val correction = Quaternion3D().also { it.makeQuat(angle, lineAbs) }
        return (correction * liv).normalized()
    }

    protected fun slantRangeFromQuaternion(liv: Quaternion3D, rAbs: Vector3D, time: Double? = null): Vector3D {
        val lineAbs = (liv * Vector3D(1.0, 0.0, 0.0)).normSafe()
        val a = astro.earth.ekvRadius
        val b = astro.earth.polarRadius
        val aa = b * b * lineAbs.x * lineAbs.x + b * b * lineAbs.y * lineAbs.y + a * a * lineAbs.z * lineAbs.z
        val bb = 2.0 * b * b * lineAbs.x * rAbs.x + 2.0 * b * b * lineAbs.y * rAbs.y + 2.0 * a * a * lineAbs.z * rAbs.z
        val cc = b * b * rAbs.x * rAbs.x + b * b * rAbs.y * rAbs.y + a * a * rAbs.z * rAbs.z - a * a * b * b
        val root = minPositiveRoot(aa, bb, cc)
            ?: throw lineOfSightDoesNotIntersectEarth(time ?: Double.NaN)
        return lineAbs * root
    }

    protected fun orientationFromVisirQuaternion(orbital: ballistics.types.OrbitalPoint, liv: Quaternion3D): Orientation {
        val ask = astro.grinvToASK(orbital)
        val ob = orbBookToOrbMatrix()
        val absOb = absToOrbBookMatrix(ask.r, ask.v)
        val absToOrbit = ob * absOb
        val lConToVisir = Quaternion3D(0.0, sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0)
        val connectedToAbs = (liv * lConToVisir.inverse()).matrix().transpose()
        val orbitToConnected = connectedToAbs * absToOrbit.transpose()
        return anglesFromOrbToCon(orbitToConnected)
    }
}
