package space.nstart.pcp.angularmotion

import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.types.BLHPoint
import ballistics.types.EarthType
import ballistics.types.OrbitalPoint
import ballistics.types.Orientation
import ballistics.utils.astro.AstronomerJ2000
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.floor
import kotlin.math.sin
import kotlin.math.sqrt

abstract class AbstractPuudCalculator(
    protected val stepper: AbstractStepper,
    protected val earthType: EarthType = EarthType.PZ90d02,
    protected val config: AngularMotionConfig = AngularMotionConfig(),
) : AngularMotionCalculator {
    protected val astro = AstronomerJ2000(earthType)

    protected fun validate(id: SurveyId) {
        require(id.duration >= 0.0) { "Длительность режима не может быть отрицательной" }
        require(config.focus > 0.0) { "Фокусное расстояние должно быть положительным" }
    }

    protected fun calculationStep(): Double =
        if (config.stepPuud in 0.1..5.0) config.stepPuud else 0.125

    protected fun pointAt(t: Double): OrbitalPoint =
        stepper.calculate(t) ?: throw AngularMotionCalculationException("Ошибка выхода на заданное время: $t")

    protected fun sdiAt(sdi: List<Double>, elapsed: Double): Double =
        if (sdi.isEmpty()) {
            -1.0
        } else {
            val index = floor(elapsed / config.stepSdi).toInt()
            sdi.getOrElse(index) { sdi.last() }
        }

    protected fun buildOeps(id: SurveyId, startTime: Double, duration: Double = id.duration): List<OepResult> =
        (0 until 4).map { index ->
            val state = id.oep.getOrNull(index) ?: false
            if (state) OepResult(true, startTime, startTime + duration) else OepResult(false)
        }

    /**
     * Поиск времени начала режима по упреждающему углу.
     * В OrbitalMotion используется EquationCalculatorSpan на [t-180; t+180]. Здесь сохранена та же идея:
     * сканирование интервала, уточнение корня бисекцией и округление вниз к дискрету времени начала.
     */
    protected fun calcTn(id: SurveyId): Double {
        val start = id.t - 180.0
        val end = id.t + 180.0
        val scanStep = 5.0
        var bestT = id.t
        var bestAbs = Double.POSITIVE_INFINITY
        var left = start
        var leftValue = equationValue(id, left)
        var bracket: Pair<Double, Double>? = null
        var t = start + scanStep

        while (t <= end + EPS) {
            val value = equationValue(id, t)
            val av = abs(value)
            if (av < bestAbs) {
                bestAbs = av
                bestT = t
            }
            if (leftValue == 0.0 || value == 0.0 || kotlin.math.sign(leftValue) != kotlin.math.sign(value)) {
                bracket = left to t
                break
            }
            left = t
            leftValue = value
            t += scanStep
        }

        val raw = bracket?.let { (a0, b0) ->
            var a = a0
            var b = b0
            var fa = equationValue(id, a)
            repeat(60) {
                val mid = (a + b) / 2.0
                val fm = equationValue(id, mid)
                if (abs(fm) < 1.0e-10) {
                    a = mid
                    b = mid
                    return@repeat
                }
                if (kotlin.math.sign(fa) == kotlin.math.sign(fm)) {
                    a = mid
                    fa = fm
                } else {
                    b = mid
                }
            }
            (a + b) / 2.0
        } ?: bestT

        return roundDownByDay(raw, config.discreteTimes)
    }

    private fun equationValue(id: SurveyId, t: Double): Double =
        orientOnPoint(t, id.b, id.l, id.h).tang - id.uprAngle

    protected fun orientOnPoint(
        t: Double,
        b: Double,
        l: Double,
        h: Double,
        routeDirectionGsk: Vector3D? = null,
    ): Orientation {
        val ka = pointAt(t)
        val targetGsk = astro.earth.blh2xyz(b, l, h)
        val targetInOrbit = pointInOrbitalFrame(ka, targetGsk)
        val orient = orientationFromOrbitalVector(targetInOrbit)

        if (routeDirectionGsk != null && routeDirectionGsk.module() > 1.0e-8) {
            orient.risk = riskFromGroundDirection(ka, orient, routeDirectionGsk)
        }
        return orient
    }

    /**
     * Подбор рысканья по физическому смыслу условия Wz/D = 0.
     *
     * Крен и тангаж уже наводят ЦЛВ на расчетную точку. Оставшаяся степень свободы —
     * поворот вокруг ЦЛВ. Переводим направление движения точки визирования с Земли в
     * ПСК при нулевом рысканье и поворачиваем ПСК так, чтобы поперечная компонента по
     * OZ стала равной нулю, то есть чтобы движение изображения шло вдоль OX.
     */
    private fun riskFromGroundDirection(
        kaGsk: OrbitalPoint,
        orientWithoutRisk: Orientation,
        routeDirectionGsk: Vector3D,
    ): Double {
        val kaAbs = astro.grinvToASK(kaGsk)
        val routeAbs = astro.grinvToASK(routeDirectionGsk, kaGsk.t)
        val routeOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
        val routeConnected = orientationMatrix(orientWithoutRisk).transpose() * routeOrbit
        val wx = routeConnected.x
        val wz = routeConnected.z
        return if (sqrt(wx * wx + wz * wz) < 1.0e-10) {
            0.0
        } else {
            normalizeAngle(atan2(-wz, wx))
        }
    }

    protected fun pointInOrbitalFrame(kaGsk: OrbitalPoint, pointGsk: Vector3D): Vector3D {
        val kaAbs = astro.grinvToASK(kaGsk)
        val pointAbs = astro.grinvToASK(pointGsk, kaGsk.t)
        val m = orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v)
        return m * (pointAbs - kaAbs.r)
    }

    protected fun orientationFromOrbitalVector(vector: Vector3D): Orientation {
        val tang = normalizeAngle(PI - atan2(vector.x, vector.y))
        val kren = normalizeAngle(atan2(vector.z, (vector.y / cos(tang).coerceAwayFromZero())) - PI)
        return Orientation(tang, kren, 0.0)
    }

    protected fun pointOnEarth(point: OrbitalPoint, orientation: Orientation): BLHPoint? {
        val ask = astro.grinvToASK(point)
        val orbitToAbs = absToOrbBookMatrix(ask.r, ask.v).transpose() * orbBookToOrbMatrix().transpose()
        val g = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) }.transpose()
        val orbitToGsk = g * orbitToAbs
        val connectedToOrbit = orientationMatrix(orientation)
        val direction = (orbitToGsk * connectedToOrbit * lineOfSightVectorInConnected()).normSafe()
        return earthIntersection(point.r, direction)
    }

    /**
     * Обязательная проверка геометрической реализуемости ориентации.
     *
     * Во всех режимах ПУУД центральная линия визирования направлена вдоль -OY ПСК.
     * Если при рассчитанной ориентации этот луч не пересекает ОЗЭ, то точка
     * визирования, W/D, СДИ и контур съемки физически не определены. В таком
     * случае расчет должен завершаться ошибкой, а не возвращать пустую точку
     * визирования или нулевые скорости.
     */
    protected fun requirePointOnEarth(point: OrbitalPoint, orientation: Orientation): BLHPoint =
        pointOnEarth(point, orientation)
            ?: throw lineOfSightDoesNotIntersectEarth(point.t, orientation)

    protected fun lineOfSightDoesNotIntersectEarth(
        t: Double,
        orientation: Orientation? = null,
    ): AngularMotionCalculationException {
        val suffix = orientation?.let {
            ", tang=${it.tang}, kren=${it.kren}, risk=${it.risk}"
        }.orEmpty()
        return AngularMotionCalculationException(
            "Линия визирования не пересекает Землю в момент t=$t$suffix",
        )
    }

    protected fun earthIntersection(rotn: Vector3D, direction: Vector3D): BLHPoint? {
        val d = direction.normSafe()
        if ((rotn + d).module() > rotn.module()) return null

        val a = astro.earth.ekvRadius
        val b = astro.earth.polarRadius
        val aa = b * b * d.x * d.x + b * b * d.y * d.y + a * a * d.z * d.z
        val bb = 2.0 * b * b * d.x * rotn.x + 2.0 * b * b * d.y * rotn.y + 2.0 * a * a * d.z * rotn.z
        val cc = b * b * rotn.x * rotn.x + b * b * rotn.y * rotn.y + a * a * rotn.z * rotn.z - a * a * b * b
        val root = minPositiveRoot(aa, bb, cc) ?: return null
        val current = rotn + d * root
        return astro.earth.xyz2blh(current)
    }

    protected fun quaternionFor(point: OrbitalPoint, orientation: Orientation): Quaternion3D {
        val ask = astro.grinvToASK(point)
        val absToOrbit = orbBookToOrbMatrix() * absToOrbBookMatrix(ask.r, ask.v)
        val orbitToConnected = orientationMatrix(orientation).transpose()
        val absToConnected = absToOrbit.transpose() * orbitToConnected
        return quaternionFromMatrix(absToConnected.transpose()).normalized()
    }

    protected fun sdiForWd(wd: Vector3D, sickle: Boolean = false): Double =
        (if (sickle) wd.z else wd.x) * config.focus

    protected fun edgeLineOfSightVectorsInConnected(): Pair<Vector3D, Vector3D>? {
        if (config.focus <= EPS || config.dlOep <= EPS) return null
        val leftOffsetMm = config.dxOep - config.dlOep / 2.0
        val rightOffsetMm = config.dxOep + config.dlOep / 2.0
        return lineOfSightByFocalPlaneZ(leftOffsetMm) to lineOfSightByFocalPlaneZ(rightOffsetMm)
    }

    protected fun edgeSdiValues(
        point: OrbitalPoint,
        orientation: Orientation,
        omegaConnected: Vector3D,
        sickle: Boolean = false,
    ): Pair<Double, Double>? {
        val (leftLine, rightLine) = edgeLineOfSightVectorsInConnected() ?: return null
        val leftSdi = sdiForWd(wd(point, orientation, omegaConnected, leftLine), sickle)
        val rightSdi = sdiForWd(wd(point, orientation, omegaConnected, rightLine), sickle)
        return leftSdi to rightSdi
    }

    private fun lineOfSightByFocalPlaneZ(offsetMm: Double): Vector3D =
        Vector3D(0.0, -config.focus, offsetMm).normSafe()

    /**
     * Аналог AbstractAISTPUUD::wd. Возвращает W/D в связанной СК для единственной центральной линии визирования.
     *
     * W — относительная скорость движения по земной поверхности точки пересечения ЦЛВ с Землей.
     * Это не скорость КА: из скорости спутника вычитается переносное вращение Земли, а затем
     * добавляется радиальная составляющая изменения наклонной дальности, чтобы точка оставалась
     * на поверхности эллипсоида. Такая схема соответствует формулам из OrbitalMotion::AbstractAISTPUUD::wd.
     */
    protected fun wd(point: OrbitalPoint, orientation: Orientation, omegaConnected: Vector3D): Vector3D =
        wd(point, orientation, omegaConnected, lineOfSightVectorInConnected())

    protected fun wd(
        point: OrbitalPoint,
        orientation: Orientation,
        omegaConnected: Vector3D,
        lineConnected: Vector3D,
    ): Vector3D {
        val ask = astro.grinvToASK(point)
        val lineInConnected = lineConnected.normSafe()
        val connectedToOrbit = orientationMatrix(orientation)
        val absToOrbBook = absToOrbBookMatrix(ask.r, ask.v)
        val orbitBookToAbs = absToOrbBook.transpose()
        val orbitToOrbitBook = orbBookToOrbMatrix().transpose()
        val absToGsk = Matrix3D().also { it.makeOzMatrix(-astro.si2000(point.t)) }

        val lineGsk = (absToGsk * orbitBookToAbs * orbitToOrbitBook * connectedToOrbit * lineInConnected).normSafe()
        if (lineGsk.module() < EPS) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)

        val earth = astro.earth
        val kalpha = 1.0 - earth.alphaEllips * earth.ekvRadius / earth.ekvRadius
        val reducedR = Vector3D(point.r.x, point.r.y, point.r.z / kalpha)
        val reducedLine = Vector3D(lineGsk.x, lineGsk.y, lineGsk.z / kalpha)
        val reducedProjection = reducedR * reducedLine
        val reducedLine2 = (reducedLine * reducedLine).coerceAtLeast(EPS)
        val underRoot = reducedProjection * reducedProjection +
            reducedLine2 * (earth.ekvRadius * earth.ekvRadius - reducedR * reducedR)
        if (underRoot < 0.0 || !underRoot.isFinite()) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)

        val range = -(reducedProjection + sqrt(underRoot)) / reducedLine2
        if (!range.isFinite() || range <= EPS) throw lineOfSightDoesNotIntersectEarth(point.t, orientation)

        val slantGsk = lineGsk * range
        val groundGsk = point.r + slantGsk
        val reducedGround = Vector3D(groundGsk.x, groundGsk.y, groundGsk.z / (kalpha * kalpha))

        var omegaGsk = connectedToOrbit * omegaConnected
        omegaGsk = orbitToOrbitBook * omegaGsk
        omegaGsk = orbitBookToAbs * omegaGsk
        omegaGsk = absToGsk * omegaGsk

        val omegaEarth = Vector3D(0.0, 0.0, earth.wEarth)
        val omegaRelativeToEarth = omegaGsk - omegaEarth
        val relativeVelocity = point.v + omegaRelativeToEarth.rem(slantGsk)
        val rangeRateCompensation = lineGsk * (-(reducedGround * relativeVelocity) / (reducedGround * lineGsk).coerceAwayFromZero())
        var wGround = relativeVelocity + rangeRateCompensation

        wGround = Matrix3D().also { it.makeOzMatrix(astro.si2000(point.t)) } * wGround
        wGround = absToOrbBook * wGround
        wGround = orbBookToOrbMatrix() * wGround
        wGround = connectedToOrbit.transpose() * wGround

        return wGround / range
    }

    protected fun routeNormalInGreenwich(b: Double, l: Double, h: Double, azimuth: Double): Vector3D {
        // Центральная линия маршрута по ТЗ задается сечением ОЗЭ плоскостью,
        // построенной в начальной точке по нормали к ОЗЭ и касательной к ОЗЭ
        // с заданным азимутом. Нормаль такой плоскости — вектор normal x tangent.
        val normal = ellipsoidNormalInGreenwich(b, l)
        val tangent = routeDirectionInGreenwich(b, l, azimuth)
        return normal.rem(tangent).normSafe()
    }

    protected fun routeDirectionInGreenwich(b: Double, l: Double, azimuth: Double): Vector3D {
        val north = northDirectionInGreenwich(b, l)
        val east = eastDirectionInGreenwich(l)
        return (north * cos(azimuth) + east * sin(azimuth)).normSafe()
    }

    protected fun routeDirectionInOrbitalFrame(id: SurveyId, t: Double): Vector3D {
        val ka = pointAt(t)
        val kaAbs = astro.grinvToASK(ka)
        val routeAbs = astro.grinvToASK(routeDirectionInGreenwich(id.b, id.l, id.azimuth), t)
        return orbBookToOrbMatrix() * absToOrbBookMatrix(kaAbs.r, kaAbs.v) * routeAbs
    }

    protected fun ellipsoidNormalInGreenwich(b: Double, l: Double): Vector3D =
        Vector3D(cos(b) * cos(l), cos(b) * sin(l), sin(b)).normSafe()

    private fun eastDirectionInGreenwich(l: Double): Vector3D =
        Vector3D(-sin(l), cos(l), 0.0).normSafe()

    private fun northDirectionInGreenwich(b: Double, l: Double): Vector3D =
        Vector3D(-sin(b) * cos(l), -sin(b) * sin(l), cos(b)).normSafe()

    protected fun endPointByAzimuth(b: Double, l: Double, azimuth: Double, centralAngle: Double): BLHPoint {
        val sinB = sin(b)
        val cosB = cos(b)
        val sinD = sin(centralAngle)
        val cosD = cos(centralAngle)
        val lat = kotlin.math.asin(sinB * cosD + cosB * sinD * cos(azimuth))
        val lon = l + atan2(sin(azimuth) * sinD * cosB, cosD - sinB * sin(lat))
        return BLHPoint(lat, normalizeAngle(lon), 0.0)
    }

    protected fun buildPoint(
        t: Double,
        orientation: Orientation,
        previous: AngularMotionPoint? = null,
        sickle: Boolean = false,
    ): AngularMotionPoint {
        val orbital = pointAt(t)
        val q = quaternionFor(orbital, orientation)
        val omega = previous?.let { omegaFromTwoQuat(it.quaternion, q, t - it.t) } ?: Vector3D()
        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 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(orientation.tang, orientation.kren, orientation.risk),
            groundPoint = ground,
            omega = omega,
            eps = eps,
            quaternion = q,
            wd = wd,
            sdi = centerSdi,
            sdiLeft = edgeSdi?.first,
            sdiRight = edgeSdi?.second,
            sdiSpread = sdiSpread,
        )
    }

    protected fun calculateTauPoints(points: List<AngularMotionPoint>): List<AngularMotionPoint> {
        if (points.size < 3 || config.tau <= 0.0) return emptyList()
        val step = calculationStep()
        val baseTime = points.first().t
        val source = points.take(minOf(6, points.size))
        val xs = source.map { it.t - baseTime }
        val tang = source.map { it.orientation.tang }
        val kren = source.map { it.orientation.kren }
        val risk = source.map { it.orientation.risk }
        val result = mutableListOf<AngularMotionPoint>()
        var d = config.tau
        var previous: AngularMotionPoint? = null
        while (d >= step - EPS) {
            val t = baseTime - d
            val x = t - baseTime
            val orientation = Orientation(
                lagrange(x, xs, tang),
                lagrange(x, xs, kren),
                lagrange(x, xs, risk),
            )
            val point = buildPoint(t, orientation, previous)
            result += point
            previous = point
            d -= step
        }
        return result.sortedBy { it.t }
    }
}
