import numpy as np

import math

from resources.LeapSDK.v53_python39 import Leap

def get_order():

    ##############

    #  i, j, k, n

    #  n = parity of axis permutation (even=False, odd=True)

    #  from https://github.com/dfelinto/blender/blob/master/source/blender/blenlib/intern/math_rotation.c

    ##############

    return [0, 1, 2, False]  # XYZ

    # return [0, 2, 1, True]   # XZY

    # return [1, 0, 2, True]   # YXZ

    # return [1, 2, 0, False]  # YZX

    # return [2, 0, 1, False]  # ZXY

    # return [2, 1, 0, True]   # ZYX

def _rot2eulsimple(rotmat):

    eul = np.zeros(3)

    if -1 < rotmat[1, 1] < 1:

        eul[1] = math.acos(rotmat[1, 1])

    if rotmat[1, 1] >= 1.0:

        eul[1] = 0

    if rotmat[1, 1] <= -1.0:

        eul[1] = np.pi

    if rotmat[0, 0] < 1 and rotmat[1, 1] > -1:

        eul[0] = math.acos(rotmat[0, 0])

    if rotmat[1, 1] >= 1.0:

        eul[0] = 0

    if rotmat[1, 1] <= -1.0:

        eul[0] = np.pi

    eul[2] = 0

    return eul

def _rot2eul(rotmat):

    ##############

    #  i, j, k, n

    #  n = parity of axis permutation (even=False, odd=True)

    #  from https://github.com/dfelinto/blender/blob/master/source/blender/blenlib/intern/math_rotation.c

    ##############

    # order = [0, 1, 2, False] # XYZ

    # order = [0, 2, 1, True] # XZY

    # order = [1, 0, 2, True] # YXZ

    # order = [1, 2, 0, False] # YZX

    # order = [2, 0, 1, False]  # ZXY

    # order = [2, 1, 0, True] # ZYX

    order = get_order()

    i = int(order[2])

    j = int(order[1])

    k = int(order[0])

    parity = order[3]

    eul1 = np.zeros(3)

    eul2 = np.zeros(3)

    cy = np.hypot(rotmat[i, i], rotmat[i, j])

    if cy > Leap.EPSILON:

        eul1[i] = math.atan2(rotmat[j, k], rotmat[k, k])

        eul1[j] = math.atan2(-rotmat[i, k], cy)

        eul1[k] = math.atan2(rotmat[i, j], rotmat[i, i])

        eul2[i] = math.atan2(-rotmat[j, k], -rotmat[k, k])

        eul2[j] = math.atan2(-rotmat[i, k], -cy)

        eul2[k] = math.atan2(-rotmat[i, j], -rotmat[i, i])

    else:

        eul1[i] = math.atan2(-rotmat[k, j], rotmat[j, j])

        eul1[j] = math.atan2(-rotmat[i, k], cy)

        eul1[k] = 0.0

        eul2 = eul1

    #  parity of axis permutation (even=False, odd=True)

    if not parity:

        eul1 = np.negative(eul1)

        eul2 = np.negative(eul2)

    # return best, which is just the one with lowest values in it

    if np.sum(np.absolute(eul1)) > np.sum(np.absolute(eul2)):

        return eul2

    return eul1

def quat2mat(quat):

    q0 = math.sqrt(2) * quat[0]

    q1 = math.sqrt(2) * quat[1]

    q2 = math.sqrt(2) * quat[2]

    q3 = math.sqrt(2) * quat[3]

    qda = q0 * q1

    qdb = q0 * q2

    qdc = q0 * q3

    qaa = q1 * q1

    qab = q1 * q2

    qac = q1 * q3

    qbb = q2 * q2

    qbc = q2 * q3

    qcc = q3 * q3

    mat = np.zeros((3, 3))

    mat[0, 0] = 1.0 - qbb - qcc

    mat[1, 0] = qdc + qab

    mat[2, 0] = -qdb + qac

    mat[0, 1] = -qdc + qab

    mat[1, 1] = 1.0 - qaa - qcc

    mat[2, 1] = qda + qbc

    mat[0, 2] = qdb + qac

    mat[1, 2] = -qda + qbc

    mat[2, 2] = 1.0 - qaa - qbb

    return mat

def vec2quat(v1, v2):

    a = np.divide(v1, np.linalg.norm(v1))

    b = np.divide(v2, np.linalg.norm(v2))

    xUnitVec = np.array([1, 0, 0])

    yUnitVec = np.array([0, 1, 0])

    dot_p = np.dot(a, b)

    if dot_p < -1 + Leap.EPSILON:

        tmpvec3 = np.cross(xUnitVec, a)

        if np.linalg.norm(tmpvec3) < Leap.EPSILON:

            tmpvec3 = np.cross(yUnitVec, a)

        tmpvec3 = np.divide(tmpvec3, np.linalg.norm(tmpvec3))

        t = np.pi * 0.5

        s = np.sin(t)

        q = np.zeros(4)

        q[0] = np.cos(t)

        q[1] = tmpvec3[0]*s

        q[2] = tmpvec3[1]*s

        q[3] = tmpvec3[2]*s

        return q

    if dot_p > 1 - Leap.EPSILON:

        return np.array([0, 0, 0, 1])

    tmpvec3 = np.cross(a, b)

    q = np.append(1 + dot_p, tmpvec3)

    return np.divide(q, np.linalg.norm(q))

def rot2quat(rotmat):

    q = np.zeros(4)

    tr = 0.25 * (1 + np.trace(rotmat))

    if tr > Leap.EPSILON:

        s = np.sqrt(tr)

        q[0] = s

        s = 1.0 / (4.0 * s)

        q[1] = (rotmat[1, 2] - rotmat[2, 1]) * s

        q[2] = (rotmat[2, 0] - rotmat[0, 2]) * s

        q[3] = (rotmat[0, 1] - rotmat[1, 0]) * s

        return normalize_quat(q)

    if rotmat[0, 0] > rotmat[1, 1] and rotmat[0, 0] > rotmat[2, 2]:

        s = 2.0 * np.sqrt(1.0 + rotmat[0, 0] - rotmat[1, 1] - rotmat[2, 2])

        q[1] = 0.25 * s

        s = 1.0 / s

        q[0] = (rotmat[1, 2] - rotmat[2, 1]) * s

        q[2] = (rotmat[1, 0] - rotmat[0, 1]) * s

        q[3] = (rotmat[2, 0] - rotmat[0, 2]) * s

        return normalize_quat(q)

    if rotmat[1, 1] > rotmat[2, 2]:

        s = 2.0 * np.sqrt(1.0 + rotmat[1, 1] - rotmat[0, 0] - rotmat[2, 2])

        q[2] = 0.25 * s

        s = 1.0 / s

        q[0] = (rotmat[2, 0] - rotmat[0, 2]) * s

        q[1] = (rotmat[1, 0] - rotmat[0, 1]) * s

        q[3] = (rotmat[2, 1] - rotmat[1, 2]) * s

        return normalize_quat(q)

    s = 2.0 * np.sqrt(1.0 + rotmat[2, 2] - rotmat[0, 0] - rotmat[1, 1])

    q[3] = 0.25 * s

    s = 1.0 / s

    q[0] = (rotmat[0, 1] - rotmat[1, 0]) * s

    q[1] = (rotmat[2, 0] - rotmat[0, 2]) * s

    q[2] = (rotmat[2, 1] - rotmat[1, 2]) * s

    return normalize_quat(q)

def normalize_quat(q):

    scal = np.sqrt(np.dot(q, q))

    if scal != 0.0:

        return np.kron(q, 1.0 / scal)

    q[1] = 1.0

    q[0] = q[2] = q[3] = 0.0

    return q

def conjugate_quat(q):

    q[1] = -q[1]

    q[2] = -q[2]

    q[3] = -q[4]

    return q

def multiply_quat(q1, q2):

    q = np.zeros(4)

    t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]

    t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]

    t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]

    q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]

    q[0] = t0

    q[1] = t1

    q[2] = t2

    return q

def vec2eul(v1, v2):

    quaternions = vec2quat(v1, v2)

    rotmat = quat2mat(quaternions)

    euler = _rot2eul(rotmat)

    return euler[0], euler[1], euler[2]

def rot2eul(rotmat):

    euler = _rot2eul(rotmat)

    return euler[0], euler[1], euler[2]

def quat_diff(q_prev, q_next):

    q_prev = conjugate_quat(q_prev)

    q_prev = np.kron(q_prev, 1.0 / np.dot(q_prev, q_prev))

    return multiply_quat(q_prev, q_next)

def quat_between_rot(rotmat_prev, rotmat_next):

    # TODO: normalize rotation matrix

    return quat_diff(rot2quat(rotmat_prev), rot2quat(rotmat_next))