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.math.Quaternion3D
import ballistics.utils.math.Vector3D

/**
 * Режим постоянной ориентации, порт ConstOrientAISTPUUD без AIST-префикса.
 *
 * Базовая логика повторяет исходный класс: уточняется время начала по упреждающему
 * углу, затем с дискретом stepPuud рассчитываются углы ориентации, точка пересечения
 * единственной ЦЛВ с Землей, кватернион, угловая скорость, W/D и СДИ.
 */
class ConstOrientPUUD(
    stepper: AbstractStepper,
    earthType: EarthType = EarthType.PZ90d02,
    config: AngularMotionConfig = AngularMotionConfig(),
) : AbstractPuudCalculator(stepper, earthType, config) {

    override fun calculate(id: SurveyId): AngularMotionResult {
        validate(id)
        val tn = calcTn(id)
        val points = calculateConst(id, tn, id.duration)
        return AngularMotionResult(
            mode = AngularMotionMode.CONST_ORIENT,
            startTime = tn,
            points = points,
            tauPoints = calculateTauPoints(points),
            oeps = buildOeps(id, tn),
        )
    }

    private fun calculateConst(id: SurveyId, tn: Double, duration: Double): List<AngularMotionPoint> {
        val step = calculationStep()

        // В режиме ConstOrient направление ЦЛВ в ОСК фиксируется на старте.
        // Это задает постоянные крен и тангаж. Оставшаяся степень свободы —
        // рысканье вокруг ЦЛВ. В отличие от геометрического варианта, где рысканье
        // выбиралось по направлению трассы, здесь рысканье интегрируется из самого
        // критерия Wz/D = 0. Такой подход совпадает по смыслу с AzimuthPUUD:
        // сначала находится угловая скорость, при которой wd.z равен нулю, затем по
        // этой скорости строится программа углового движения.
        val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h)
        val tang = fixedClvOrientation.tang
        val kren = fixedClvOrientation.kren
        var risk = buildConstSample(id, tn, step, fixedClvOrientation).orientation.risk

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

        while (elapsed <= duration + EPS) {
            val orientation = Orientation(tang, kren, normalizeAngle(risk))
            val omega = omegaForWdzZero(t, orientation)
            val point = buildConstPoint(t, orientation, omega, previous)
            result += point
            previous = point

            if (elapsed + step <= duration + EPS) {
                risk = integrateRiskRK4(t, risk, step, tang, kren)
            }
            elapsed += step
            t += step
        }

        return result
    }

    /**
     * В AzimuthPUUD условие wd.z = 0 зашито непосредственно в формулы угловой
     * скорости визирной системы. Для ConstOrient оставшаяся степень свободы только
     * одна — производная рысканья. Поэтому в каждой точке решаем линейное уравнение
     * по скорости рысканья:
     *
     *     wd.z(t, risk, riskDot) = 0.
     *
     * Функция wd линейна по omega, а omega линейна по riskDot, поэтому достаточно
     * двух расчетов: при riskDot = 0 и при riskDot = 1 рад/с.
     */
    private fun riskRateForWdzZero(t: Double, orientation: Orientation): Double {
        val point = pointAt(t)
        val omega0 = omegaForRiskRate(t, orientation, 0.0)
        val wz0 = wd(point, orientation, omega0).z
        val omega1 = omegaForRiskRate(t, orientation, 1.0)
        val wz1 = wd(point, orientation, omega1).z
        val denom = wz1 - wz0
        return if (!denom.isFinite() || kotlin.math.abs(denom) < WDZ_SOLVER_EPS) {
            0.0
        } else {
            -wz0 / denom
        }
    }

    private fun omegaForWdzZero(t: Double, orientation: Orientation): Vector3D =
        omegaForRiskRate(t, orientation, riskRateForWdzZero(t, orientation))

    /**
     * Численно строит мгновенную угловую скорость ПСК при заданной скорости рысканья.
     * Малый внутренний шаг нужен только для дифференцирования кватерниона; он не
     * задает дискрет выдачи ПУУД.
     */
    private fun omegaForRiskRate(t: Double, orientation: Orientation, riskRate: Double): Vector3D {
        val dt = OMEGA_DERIVATIVE_STEP
        val q0 = quaternionFor(pointAt(t), orientation)
        val nextOrientation = Orientation(
            orientation.tang,
            orientation.kren,
            normalizeAngle(orientation.risk + riskRate * dt),
        )
        val q1 = quaternionFor(pointAt(t + dt), nextOrientation)
        return omegaFromTwoQuat(q0, q1, dt)
    }

    private fun integrateRiskRK4(t: Double, risk: Double, step: Double, tang: Double, kren: Double): Double {
        fun f(time: Double, value: Double): Double =
            riskRateForWdzZero(time, Orientation(tang, kren, normalizeAngle(value)))

        val k1 = f(t, risk)
        val k2 = f(t + step / 2.0, risk + k1 * step / 2.0)
        val k3 = f(t + step / 2.0, risk + k2 * step / 2.0)
        val k4 = f(t + step, risk + k3 * step)
        return normalizeAngle(risk + step * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0)
    }

    private fun buildConstPoint(
        t: Double,
        orientation: Orientation,
        omega: Vector3D,
        previous: AngularMotionPoint?,
    ): AngularMotionPoint {
        val orbital = pointAt(t)
        val ground = requirePointOnEarth(orbital, orientation)
        val q = quaternionFor(orbital, orientation)
        val eps = previous?.let { if (kotlin.math.abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() }
            ?: Vector3D()
        val wd = wd(orbital, orientation, omega)
        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 = sdiForWd(wd),
        )
    }

    private fun buildConstSample(
        id: SurveyId,
        t: Double,
        step: Double,
        fixedClvOrientation: Orientation,
    ): ConstSample {
        val orbital = pointAt(t)
        val currentGround = requirePointOnEarth(orbital, fixedClvOrientation)
        val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround)
        val orientation = orientOnPoint(
            t,
            currentGround.lat,
            currentGround.long,
            currentGround.h,
            routeDirectionGsk,
        )
        val ground = requirePointOnEarth(orbital, orientation)
        val quaternion = quaternionFor(orbital, orientation)
        return ConstSample(t, orbital, orientation, ground, quaternion)
    }

    private fun pointOnFixedClv(t: Double, fixedClvOrientation: Orientation): BLHPoint? =
        pointOnEarth(pointAt(t), fixedClvOrientation)

    private fun fixedClvGroundDirection(
        t: Double,
        step: Double,
        fixedClvOrientation: Orientation,
        currentGround: BLHPoint,
    ): Vector3D {
        val previousGround = pointOnFixedClv(t - step, fixedClvOrientation)
        val nextGround = pointOnFixedClv(t + step, fixedClvOrientation)

        return when {
            previousGround != null && nextGround != null -> groundDirection(previousGround, nextGround)
            nextGround != null -> groundDirection(currentGround, nextGround)
            previousGround != null -> groundDirection(previousGround, currentGround)
            else -> Vector3D()
        }
    }

    private fun groundDirection(from: BLHPoint, to: BLHPoint): Vector3D {
        val first = astro.earth.blh2xyz(from.lat, from.long, from.h)
        val second = astro.earth.blh2xyz(to.lat, to.long, to.h)
        return second - first
    }

    private data class ConstSample(
        val t: Double,
        val orbitalPoint: OrbitalPoint,
        val orientation: Orientation,
        val groundPoint: BLHPoint?,
        val quaternion: Quaternion3D,
    )

    private companion object {
        const val WDZ_SOLVER_EPS = 1.0e-14
        const val OMEGA_DERIVATIVE_STEP = 1.0e-2
    }
}
