package space.nstart.pcp.angularmotion

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.acos
import kotlin.math.atan2
import kotlin.math.cos
import kotlin.math.floor
import kotlin.math.max
import kotlin.math.min
import kotlin.math.sin
import kotlin.math.sqrt

internal const val EPS = 1.0e-12

internal fun Vector3D.normSafe(): Vector3D {
    val m = module()
    return if (m < EPS || !m.isFinite()) Vector3D() else Vector3D(x / m, y / m, z / m)
}

internal operator fun Vector3D.div(k: Double): Vector3D = Vector3D(x / k, y / k, z / k)

internal fun Vector3D.copyOf(): Vector3D = Vector3D(x, y, z)

internal fun Quaternion3D.copyOf(): Quaternion3D = Quaternion3D(q0, q1, q2, q3)

internal fun Quaternion3D.normalized(): Quaternion3D {
    val n = norm()
    return if (n < EPS || !n.isFinite()) Quaternion3D(1.0, 0.0, 0.0, 0.0) else Quaternion3D(q0 / n, q1 / n, q2 / n, q3 / n)
}

internal fun Quaternion3D.inverse(): Quaternion3D = normalized().opposite()

internal fun Quaternion3D.scaled(k: Double): Quaternion3D = Quaternion3D(q0 * k, q1 * k, q2 * k, q3 * k)

internal fun Matrix3D.copyOf(): Matrix3D = Matrix3D(first.copyOf(), second.copyOf(), third.copyOf())

internal fun orientationMatrix(orientation: Orientation): Matrix3D {
    val tang = orientation.tang
    val kren = orientation.kren
    val risk = orientation.risk
    return Matrix3D(
        Vector3D(
            cos(tang) * cos(risk) - sin(tang) * sin(kren) * sin(risk),
            -sin(tang) * cos(kren),
            cos(tang) * sin(risk) + sin(tang) * sin(kren) * cos(risk),
        ),
        Vector3D(
            sin(tang) * cos(risk) + cos(tang) * sin(kren) * sin(risk),
            cos(tang) * cos(kren),
            sin(tang) * sin(risk) - cos(tang) * sin(kren) * cos(risk),
        ),
        Vector3D(
            -cos(kren) * sin(risk),
            sin(kren),
            cos(kren) * cos(risk),
        ),
    )
}

internal fun anglesFromOrbToCon(m: Matrix3D): Orientation {
    val mt = m.transpose()
    var kren = asinClamped(mt.third.y)
    val cosKren = cos(kren).coerceAwayFromZero()
    var tang = atan2(-mt.first.y / cosKren, (mt.second.y / cosKren).coerceIn(-1.0, 1.0))
    if (tang > PI) tang -= 2.0 * PI
    var risk = atan2(-mt.third.x / cosKren, mt.third.z / cosKren)
    if (risk > PI) risk -= 2.0 * PI
    return Orientation(normalizeAngle(tang), normalizeAngle(kren), normalizeAngle(risk))
}

internal fun asinClamped(v: Double): Double = kotlin.math.asin(v.coerceIn(-1.0, 1.0))

internal fun Double.coerceAwayFromZero(): Double =
    when {
        abs(this) >= EPS -> this
        this < 0.0 -> -EPS
        else -> EPS
    }

internal fun normalizeAngle(value: Double): Double {
    var x = value
    while (x > PI) x -= 2.0 * PI
    while (x < -PI) x += 2.0 * PI
    return x
}

internal fun normalizePositiveAngle(value: Double): Double {
    var x = value
    while (x < 0.0) x += 2.0 * PI
    while (x >= 2.0 * PI) x -= 2.0 * PI
    return x
}

internal fun rotationZ(angle: Double): Matrix3D = Matrix3D().also { it.makeOzMatrix(angle) }

internal fun rotationX(angle: Double): Matrix3D = Matrix3D().also { it.makeOxMatrix(angle) }

internal fun rotationY(angle: Double): Matrix3D = Matrix3D().also { it.makeOyMatrix(angle) }

/**
 * Матрица перехода от Абсолютной СК к орбитальной книжной СК по той же схеме,
 * которая уже используется в ballistics.mpl.OrientOnPointCalculator.
 */
internal fun absToOrbBookMatrix(rAbs: Vector3D, vAbs: Vector3D): Matrix3D {
//    val c = Matrix3D()
//    c.second = rAbs.normSafe()
//    c.third = (vAbs.rem(rAbs)).normSafe()
//    c.first = c.second.rem(c.third).normSafe()
    val c = rAbs.rem(vAbs)
    val dummy = c.rem(rAbs)
    val modC = c.module()
    val modAbsRKA = rAbs.module()
    val m = Matrix3D(
        Vector3D( dummy.x/modC/modAbsRKA,c.x/modC, rAbs.x/modAbsRKA),
        Vector3D( dummy.y/modC/modAbsRKA,c.y/modC, rAbs.y/modAbsRKA),
        Vector3D( dummy.z/modC/modAbsRKA,c.z/modC, rAbs.z/modAbsRKA),

    )
    return m.transpose()
}

/** Матрица из OrbitalMotion::TLowOrbit::OrbBookToOrb. */
internal fun orbBookToOrbMatrix(): Matrix3D = Matrix3D(
    Vector3D(1.0, 0.0, 0.0),
    Vector3D(0.0, 0.0, 1.0),
    Vector3D(0.0, -1.0, 0.0),
)

internal fun conToVisirMatrix(): Matrix3D = Matrix3D(
    Vector3D(0.0, -1.0, 0.0),
    Vector3D(-1.0, 0.0, 0.0),
    Vector3D(0.0, 0.0, -1.0),
)

internal fun conToOpticMatrix(): Matrix3D = Matrix3D(
    Vector3D(0.0, 1.0, 0.0),
    Vector3D(1.0, 0.0, 0.0),
    Vector3D(0.0, 0.0, -1.0),
)

internal fun lineOfSightVectorInConnected(): Vector3D {
    // У спутника используется единственная центральная линия визирования.
    // В программной/связанной СК при нулевых углах +OY направлена от Земли,
    // поэтому ЦЛВ, направленная из центра КА к Земле, задается вектором (0, -1, 0).
    return Vector3D(0.0, -1.0, 0.0)
}

internal fun quaternionFromMatrix(m: Matrix3D): Quaternion3D = Quaternion3D().also { it.fromMatrixStanley(m) }.normalized()

internal fun quaternionFromEuler(tang: Double, kren: Double, risk: Double): Quaternion3D =
    quaternionFromMatrix(orientationMatrix(Orientation(tang, kren, risk))).normalized()

internal fun omegaFromTwoQuat(q1: Quaternion3D, q2: Quaternion3D, step: Double): Vector3D {
    if (abs(step) < EPS) return Vector3D()
    val q1n = q1.normalized()
    val q2n = alignQuaternion(q1n, q2.normalized())
    val dq = (q2n - q1n).scaled(1.0 / step)
    return omegaFromQuaternionDerivative(q1n, dq)
}

internal fun omegaFromThreeQuat(
    qPrevious: Quaternion3D,
    qCurrent: Quaternion3D,
    qNext: Quaternion3D,
    step: Double,
): Vector3D {
    if (abs(step) < EPS) return Vector3D()
    val current = qCurrent.normalized()
    val previous = alignQuaternion(current, qPrevious.normalized())
    val next = alignQuaternion(current, qNext.normalized())
    val dq = (next - previous).scaled(1.0 / (2.0 * step))
    return omegaFromQuaternionDerivative(current, dq)
}

private fun alignQuaternion(reference: Quaternion3D, candidate: Quaternion3D): Quaternion3D {
    val dot = reference.q0 * candidate.q0 + reference.q1 * candidate.q1 +
        reference.q2 * candidate.q2 + reference.q3 * candidate.q3
    return if (dot < 0.0) candidate.scaled(-1.0) else candidate
}

private fun omegaFromQuaternionDerivative(q: Quaternion3D, dq: Quaternion3D): Vector3D {
    val l1 = Vector3D(q.q1, q.q2, q.q3)
    val l2 = Vector3D(dq.q1, dq.q2, dq.q3)
    return (l1 * (-dq.q0) + l2 * q.q0 - (l1.rem(l2))) * 2.0
}

internal fun integrateQuaternionRK4(
    time: Double,
    q: Quaternion3D,
    step: Double,
    omegaProvider: (Double, Quaternion3D) -> Vector3D,
): Quaternion3D {
    fun derivative(t: Double, qq: Quaternion3D): Quaternion3D {
        val n2 = qq.q0 * qq.q0 + qq.q1 * qq.q1 + qq.q2 * qq.q2 + qq.q3 * qq.q3
        val omega = omegaProvider(t, qq.normalized())
        return Quaternion3D(
            (-omega.x * qq.q1 - omega.y * qq.q2 - omega.z * qq.q3 - qq.q0 * (n2 - 1.0)) / 2.0,
            (omega.x * qq.q0 + omega.z * qq.q2 - omega.y * qq.q3 - qq.q1 * (n2 - 1.0)) / 2.0,
            (omega.y * qq.q0 + omega.x * qq.q3 - omega.z * qq.q1 - qq.q2 * (n2 - 1.0)) / 2.0,
            (omega.z * qq.q0 + omega.y * qq.q1 - omega.x * qq.q2 - qq.q3 * (n2 - 1.0)) / 2.0,
        )
    }

    val k1 = derivative(time, q)
    val k2 = derivative(time + step / 2.0, q + k1.scaled(step / 2.0))
    val k3 = derivative(time + step / 2.0, q + k2.scaled(step / 2.0))
    val k4 = derivative(time + step, q + k3.scaled(step))

    return (q + (k1 + k2.scaled(2.0) + k3.scaled(2.0) + k4).scaled(step / 6.0)).normalized()
}

internal fun roundDownByDay(value: Double, step: Double): Double {
    if (step <= 0.0) return value
    val day = floor(value / 86400.0) * 86400.0
    val seconds = value - day
    return day + floor(seconds / step) * step
}

internal fun lagrange(x: Double, xs: List<Double>, ys: List<Double>): Double {
    require(xs.size == ys.size && xs.isNotEmpty())
    var result = 0.0
    for (i in xs.indices) {
        var term = ys[i]
        for (j in xs.indices) {
            if (i != j) {
                val denom = xs[i] - xs[j]
                if (abs(denom) >= EPS) {
                    term *= (x - xs[j]) / denom
                }
            }
        }
        result += term
    }
    return result
}

internal fun minPositiveRoot(a: Double, b: Double, c: Double): Double? {
    val d = b * b - 4.0 * a * c
    if (d < 0.0 || abs(a) < EPS) return null
    val sqrtD = sqrt(d)
    val x1 = (-b + sqrtD) / (2.0 * a)
    val x2 = (-b - sqrtD) / (2.0 * a)
    val candidates = listOf(x1, x2).filter { it.isFinite() && it > 0.0 }
    return candidates.minOrNull()
}

internal fun clampUnit(v: Double): Double = max(-1.0, min(1.0, v))
