"""标量在前的单位姿态四元数，局部坐标到世界坐标，角度为弧度。"""
import math


def normalize(q):
    length = math.hypot(*q)
    if length == 0:
        raise ValueError("零四元数没有单位方向")
    return [x / length for x in q]


def conjugate(q):
    return [q[0], -q[1], -q[2], -q[3]]


def mul(p, q):
    a, x, y, z = p
    b, u, v, w = q
    return [a*b - x*u - y*v - z*w,
            a*u + b*x + y*w - z*v,
            a*v + b*y + z*u - x*w,
            a*w + b*z + x*v - y*u]


def rotate(q, x):
    """单位四元数的共轭旋转，返回向量部分。"""
    return mul(mul(q, [0.0] + list(x)), conjugate(q))[1:]


def from_rotvec(phi):
    theta = math.hypot(*phi)
    theta2 = theta * theta
    scale = (0.5 - theta2/48 + theta2*theta2/3840
             if theta < 1e-6 else math.sin(theta/2)/theta)
    return [math.cos(theta/2)] + [scale*x for x in phi]


def to_rotvec(q):
    """主值转角在 [0, pi]；严格半周时最大向量分量取正。"""
    q = normalize(q)
    if abs(q[0]) < 1e-14:
        largest = max(range(1, 4), key=lambda k: abs(q[k]))
        if q[largest] < 0:
            q = [-x for x in q]
        q[0] = 0.0
    elif q[0] < 0:
        q = [-x for x in q]
    sine = math.hypot(*q[1:])
    if sine < 1e-8:
        return [2*x for x in q[1:]]
    angle = 2 * math.atan2(sine, q[0])
    return [angle*x/sine for x in q[1:]]


def to_matrix(q):
    """单位四元数转九元素行优先旋转矩阵。"""
    s, x, y, z = q
    return [s*s+x*x-y*y-z*z, 2*(x*y-s*z), 2*(x*z+s*y),
            2*(x*y+s*z), s*s-x*x+y*y-z*z, 2*(y*z-s*x),
            2*(x*z-s*y), 2*(y*z+s*x), s*s-x*x-y*y+z*z]


def from_matrix(r):
    """输入为合法旋转；先恢复绝对值最大的四元数分量。"""
    candidates = [1+r[0]+r[4]+r[8], 1+r[0]-r[4]-r[8],
                  1-r[0]+r[4]-r[8], 1-r[0]-r[4]+r[8]]
    k = max(range(4), key=lambda i: candidates[i])
    q = [0.0] * 4
    q[k] = 0.5 * math.sqrt(max(0.0, candidates[k]))
    denominator = 4*q[k]
    products = [[0, r[7]-r[5], r[2]-r[6], r[3]-r[1]],
                [r[7]-r[5], 0, r[1]+r[3], r[2]+r[6]],
                [r[2]-r[6], r[1]+r[3], 0, r[5]+r[7]],
                [r[3]-r[1], r[2]+r[6], r[5]+r[7], 0]]
    for j in range(4):
        if j != k:
            q[j] = products[k][j] / denominator
    return normalize(q)


def derivative(q, omega_body):
    return [0.5*x for x in mul(q, [0.0] + list(omega_body))]


def step_body(q, omega_body, h):
    return normalize(mul(normalize(q), from_rotvec([h*x for x in omega_body])))


def step_world(q, omega_world, h):
    return normalize(mul(from_rotvec([h*x for x in omega_world]), normalize(q)))


def align(q0, q1):
    """归一化并选取短弧端点。"""
    a, b = normalize(q0), normalize(q1)
    if sum(x*y for x, y in zip(a, b)) < 0:
        b = [-x for x in b]
    return a, b


def nlerp(q0, q1, t):
    a, b = align(q0, q1)
    return normalize([(1-t)*x+t*y for x, y in zip(a, b)])


def slerp(q0, q1, t):
    """[0, 1] 内的最短球面插值；近重合时采用归一化线性插值。"""
    a, b = align(q0, q1)
    cosine = max(0.0, min(1.0, sum(x*y for x, y in zip(a, b))))
    if cosine > 1-1e-12:
        return nlerp(a, b, t)
    angle = math.acos(cosine)
    u = math.sin((1-t)*angle) / math.sin(angle)
    v = math.sin(t*angle) / math.sin(angle)
    return normalize([u*x+v*y for x, y in zip(a, b)])