# Path: pyem/geom/geom_numba.py
# @numba.guvectorize(["void(float64[:], float64[:], float64[:])"],
#                    "(m),(m)->(m)", nopython=True, cache=False)
# def cross3(u, v, w):
#     w[0] = u[1] * v[2] - u[2] * v[1]
#     w[1] = u[2] * v[0] - u[0] * v[2]
#     w[2] = u[0] * v[1] - u[1] * v[0]


import numpy as np
from .geom_numba import cross3
#     if np.abs(sin_half_theta) < 1E-12:
#         return (q1 + q2) / 2
#     a = np.sin((1 - t) * half_theta) / sin_half_theta
#     b = np.sin(t * half_theta) / sin_half_theta
#     return q1 * a + q2 * b


def normq(q, mu=None):
    q = (q.T/np.linalg.norm(q, axis=1)).T
    if mu is not None:
        ang = np.dot(q, mu) < 0
        q[ang] = -q[ang]
    return q


def meanq(q, w=None):
    if w is None:
        return np.linalg.eigh(np.einsum('ij,ik->...jk', q, q))[1][:, -1]
    else:
        return np.linalg.eigh(np.einsum('ij,ik,i->...jk', q, q, w))[1][:, -1]


def pdistq(q1, q2=None):
    if q2 is None:
        q2 = q1
    d = np.abs(q1.dot(q2.T))
    # dots[dots > 1.0] = 1.0
    np.clip(d, 0, 1.0, out=d)
    np.arccos(d, out=d)
    d *= 2

# Path: pyem/geom/geom_numba.py
# @numba.guvectorize(["void(float64[:], float64[:], float64[:])"],
#                    "(m),(m)->(m)", nopython=True, cache=False)
# def cross3(u, v, w):
#     w[0] = u[1] * v[2] - u[2] * v[1]
#     w[1] = u[2] * v[0] - u[0] * v[2]
#     w[2] = u[0] * v[1] - u[1] * v[0]


# Path: pyem/geom/quat.py
import numpy as np
from .geom_numba import cross3

#     if np.abs(sin_half_theta) < 1E-12:
#         return (q1 + q2) / 2
#     a = np.sin((1 - t) * half_theta) / sin_half_theta
#     b = np.sin(t * half_theta) / sin_half_theta
#     return q1 * a + q2 * b


def normq(q, mu=None):
    q = (q.T/np.linalg.norm(q, axis=1)).T
    if mu is not None:
        ang = np.dot(q, mu) < 0
        q[ang] = -q[ang]
    return q


def meanq(q, w=None):
    if w is None:
        return np.linalg.eigh(np.einsum('ij,ik->...jk', q, q))[1][:, -1]
    else:
        return np.linalg.eigh(np.einsum('ij,ik,i->...jk', q, q, w))[1][:, -1]


def pdistq(q1, q2=None):
    if q2 is None:
        q2 = q1
    d = np.abs(q1.dot(q2.T))
    # dots[dots > 1.0] = 1.0
    np.clip(d, 0, 1.0, out=d)
    np.arccos(d, out=d)
    d *= 2

    d_over_pi_half = d > np.pi/2