package ballistics.utils.math

import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
import kotlin.math.sqrt

class Quaternion3D(
    var q0: Double,
    var q1: Double,
    var q2: Double,
    var q3: Double,
) {
    constructor() : this(0.0, 0.0, 0.0, 0.0)

    fun makeQuat(
        angle: Double,
        v: Vector3D,
    ) {
        var vv = v.basis()

        q0 = cos(angle / 2.0)
        q1 = vv.x * sin(angle / 2.0)
        q2 = vv.y * sin(angle / 2.0)
        q3 = vv.z * sin(angle / 2.0)

        makeBasis()
    }

    /*!
    норма кватерниона
     */
    fun norm() = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)

    fun makeBasis() {
        var n = norm()
        q0 /= n
        q1 /= n
        q2 /= n
        q3 /= n
    }

    fun basis(): Quaternion3D {
        var n = norm()
        return Quaternion3D(q0 / n, q1 / n, q2 / n, q3 / n)
    }

    fun makeOpposite() {
        var n = norm()
        q0 /= n
        q1 /= -n
        q2 /= -n
        q3 /= -n
    }

    fun opposite(): Quaternion3D {
        var n = norm()
        return Quaternion3D(q0 / n, -q1 / n, -q2 / n, -q3 / n)
    }

    fun module() = acos(basis().q0) * 2.0

    fun matrix(): Matrix3D {
        var x2 = q1 + q1
        var y2 = q2 + q2
        var z2 = q3 + q3

        var xx = q1 * x2
        var yy = q2 * y2
        var wx = q0 * x2

        var xy = q1 * y2
        var yz = q2 * z2
        var wy = q0 * y2

        var xz = q1 * z2
        var zz = q3 * z2
        var wz = q0 * z2

        return Matrix3D(
            Vector3D(1.0 - (yy + zz), xy + wz, xz - wy),
            Vector3D(xy - wz, 1.0 - (xx + zz), yz + wx),
            Vector3D(xz + wy, yz - wx, 1.0 - (xx + yy)),
        ).transpose()
    }

    fun fromMatrixStanley(m: Matrix3D) {
        val t = m.first.x + m.second.y + m.third.z

        val valueq0 = (1.0 + t) / 4.0
        val valueq1 = (1.0 + 2.0 * m.first.x - t) / 4.0
        val valueq2 = (1.0 + 2.0 * m.second.y - t) / 4.0
        val valueq3 = (1.0 + 2.0 * m.third.z - t) / 4.0

        if ((valueq0 >= valueq1) && (valueq0 >= valueq2) && (valueq0 >= valueq3)) {
            q0 = sqrt(valueq0)
            q1 = (m.second.z - m.third.y) / (4.0 * q0)
            q2 = (m.third.x - m.first.z) / (4.0 * q0)
            q3 = (m.first.y - m.second.x) / (4.0 * q0)
        }

        if ((valueq1 >= valueq0) && (valueq1 >= valueq2) && (valueq1 >= valueq3)) {
            q1 = sqrt(valueq1)
            q0 = (m.second.z - m.third.y) / (4.0 * q1)
            q2 = (m.first.y + m.second.x) / (4.0 * q1)
            q3 = (m.third.x + m.first.z) / (4.0 * q1)
        }

        if ((valueq2 >= valueq0) && (valueq2 >= valueq1) && (valueq2 >= valueq3)) {
            q2 = sqrt(valueq2)
            q0 = (m.third.x - m.first.z) / (4.0 * q2)
            q1 = (m.first.y + m.second.x) / (4.0 * q2)
            q3 = (m.second.z + m.third.y) / (4.0 * q2)
        }

        if ((valueq3 >= valueq1) && (valueq3 >= valueq2) && (valueq3 >= valueq0)) {
            q3 = sqrt(valueq3)
            q0 = (m.first.y - m.second.x) / (4.0 * q3)
            q1 = (m.third.x + m.first.z) / (4.0 * q3)
            q2 = (m.second.z + m.third.y) / (4.0 * q3)
        }

        makeBasis()
        makeOpposite()
    }

    operator fun times(v: Vector3D): Vector3D {
        var p = opposite()
        var r = Vector3D()

        var a = (q0 + q1) * (v.x)
        var b = (q3 - q2) * (v.y - v.z)
        var c = (q1 - q0) * (v.y + v.z)
        var d = (q2 + q3) * (v.x)
        var e = (q1 + q3) * (v.x + v.y)
        var f = (q1 - q3) * (v.x - v.y)
        var g = (q0 + q2) * (-v.z)
        var h = (q0 - q2) * (v.z)

        var qq0 = b + (-e - f + g + h) * 0.5
        var qq1 = a - (e + f + g + h) * 0.5
        var qq2 = -c + (e - f + g - h) * 0.5
        var qq3 = -d + (e - f - g + h) * 0.5

        a = (qq0 + qq1) * (p.q0 + p.q1)
        c = (qq1 - qq0) * (p.q2 + p.q3)
        d = (qq2 + qq3) * (p.q1 - p.q0)
        e = (qq1 + qq3) * (p.q1 + p.q2)
        f = (qq1 - qq3) * (p.q1 - p.q2)
        g = (qq0 + qq2) * (p.q0 - p.q3)
        h = (qq0 - qq2) * (p.q0 + p.q3)

        r.x = a - (e + f + g + h) * 0.5
        r.y = -c + (e - f + g - h) * 0.5
        r.z = -d + (e - f - g + h) * 0.5

        return r
    }

    operator fun times(other: Quaternion3D): Quaternion3D {
        var a = (q0 + q1) * (other.q0 + other.q1)
        var b = (q3 - q2) * (other.q2 - other.q3)
        var c = (q1 - q0) * (other.q2 + other.q3)
        var d = (q2 + q3) * (other.q1 - other.q0)
        var e = (q1 + q3) * (other.q1 + other.q2)
        var f = (q1 - q3) * (other.q1 - other.q2)
        var g = (q0 + q2) * (other.q0 - other.q3)
        var h = (q0 - q2) * (other.q0 + other.q3)

        return Quaternion3D(
            b + (-e - f + g + h) * 0.5,
            a - (e + f + g + h) * 0.5,
            -c + (e - f + g - h) * 0.5,
            -d + (e - f - g + h) * 0.5,
        )
    }

    operator fun plus(other: Quaternion3D): Quaternion3D {
        return Quaternion3D(
            q0 + other.q0,
            q1 + other.q1,
            q2 + other.q2,
            q3 + other.q3,
        )
    }

    operator fun minus(other: Quaternion3D): Quaternion3D {
        return Quaternion3D(
            q0 - other.q0,
            q1 - other.q1,
            q2 - other.q2,
            q3 - other.q3,
        )
    }
}
