package space.nstart.pcp.angularmotion

import ballistics.orbitalPoints.timeStepper.AbstractStepper
import ballistics.types.EarthType
import ballistics.utils.math.Matrix3D
import ballistics.utils.math.Quaternion3D
import ballistics.utils.math.Vector3D

/**
 * Режим плавной реализации СДИ, порт SmoothSDIAISTPUUD без AIST-префикса.
 *
 * Режим наследует азимутальную постановку маршрута, но переопределяет расчет
 * абсолютной угловой скорости в визирной СК по схеме OwnCornerSpeedLevelSpeed.
 */
class SmoothSDIPUUD(
    stepper: AbstractStepper,
    earthType: EarthType = EarthType.PZ90d02,
    config: AngularMotionConfig = AngularMotionConfig(),
) : AzimuthPUUD(stepper, earthType, config, sickle = false) {

    override fun calculate(id: SurveyId): AngularMotionResult {
        validate(id)
        if (id.sdi.isEmpty()) {
            throw AngularMotionCalculationException("Для режима SmoothSDI требуется массив СДИ")
        }
        val tn = calcTn(id)
        val points = calculateAzimuth(id, tn, id.duration)
        return AngularMotionResult(
            mode = AngularMotionMode.SMOOTH_SDI,
            startTime = tn,
            points = points,
            tauPoints = calculateTauPoints(points),
            oeps = buildOeps(id, tn),
        )
    }

    override 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 visToCon = conToVisirMatrix().transpose()
        val visToOptic = visToCon * conToOpticMatrix().transpose()
        val velocityOptic = visToOptic * (liv.inverse() * relVelocityAbs)
        val range = di.module().coerceAtLeast(EPS)
        val vsOptic = velocityOptic / range

        val miv = liv.matrix().transpose()
        val currentAbs = rAbs + di
        val currentGsk = astro.askToGrinvich(currentAbs, time)
        val mih = horizontalGeodesicToAbs(currentGsk, time, routeNormalGsk)
        val chv = miv * mih.transpose()
        val perspective = visToOptic * chv
        val denom = perspective.first.x.coerceAwayFromZero()

        val programmed = Vector3D(
            -vsOptic.y * perspective.third.x / denom,
            -vsOptic.z,
            vsOptic.y - sdi / config.focus,
        )
        val omegaInVisir = visToOptic * programmed + (liv.inverse() * omegaEarth)
        return alignEdgeSdi(time, liv, omegaInVisir, sdi)
    }

    /**
     * Вариант №3 требует не только Wz/D = 0 и заданную СДИ на центральной ЦЛВ,
     * но и минимизацию изменения СДИ по ширине полосы. Центральная ЦЛВ не чувствительна
     * к вращению вокруг себя, зато крайние лучи линейки ОЭП чувствительны. Поэтому
     * подбираем добавку к компоненте угловой скорости вокруг ЦЛВ так, чтобы СДИ на
     * левом и правом краях была максимально близка к заданной.
     */
    private fun alignEdgeSdi(
        time: Double,
        liv: Quaternion3D,
        omegaInVisir: Vector3D,
        targetSdi: Double,
    ): Vector3D {
        if (config.dlOep <= EPS || config.focus <= EPS) return omegaInVisir

        val orbital = pointAt(time)
        val orientation = orientationFromVisirQuaternion(orbital, liv)

        fun edgeSdiFor(omegaVisir: Vector3D): Pair<Double, Double>? =
            edgeSdiValues(orbital, orientation, visirToConnected(omegaVisir), sickle = false)

        val baseEdges = edgeSdiFor(omegaInVisir) ?: return omegaInVisir
        val probeOmega = Vector3D(omegaInVisir.x + LINE_SPIN_PROBE, omegaInVisir.y, omegaInVisir.z)
        val probeEdges = edgeSdiFor(probeOmega) ?: return omegaInVisir

        val leftK = (probeEdges.first - baseEdges.first) / LINE_SPIN_PROBE
        val rightK = (probeEdges.second - baseEdges.second) / LINE_SPIN_PROBE
        val denom = leftK * leftK + rightK * rightK
        if (denom < EPS || !denom.isFinite()) return omegaInVisir

        val correction = -((baseEdges.first - targetSdi) * leftK + (baseEdges.second - targetSdi) * rightK) / denom
        if (!correction.isFinite()) return omegaInVisir

        return Vector3D(
            omegaInVisir.x + correction.coerceIn(-MAX_LINE_SPIN_CORRECTION, MAX_LINE_SPIN_CORRECTION),
            omegaInVisir.y,
            omegaInVisir.z,
        )
    }

    private fun horizontalGeodesicToAbs(
        pointGsk: Vector3D,
        time: Double,
        routeNormalGsk: Vector3D?,
    ): Matrix3D {
        val blh = astro.earth.xyz2blh(pointGsk)
        val upGsk = ellipsoidNormalInGreenwich(blh.lat, blh.long)
        val alongGsk = routeNormalGsk
            ?.let { upGsk.rem(it) }
            ?.normSafe()
            ?.takeIf { it.module() >= EPS }
            ?: Vector3D(
                -kotlin.math.sin(blh.lat) * kotlin.math.cos(blh.long),
                -kotlin.math.sin(blh.lat) * kotlin.math.sin(blh.long),
                kotlin.math.cos(blh.lat),
            ).normSafe()
        val crossGsk = upGsk.rem(alongGsk).normSafe()
        val gskToAbs = Matrix3D().also { it.makeOzMatrix(astro.si2000(time)) }
        return Matrix3D(
            gskToAbs * alongGsk,
            gskToAbs * crossGsk,
            gskToAbs * upGsk,
        )
    }
    private companion object {
        const val LINE_SPIN_PROBE = 1.0e-5
        const val MAX_LINE_SPIN_CORRECTION = 0.05
    }

}
