"""Module for Euler transforms.

Best to use generic routines at the bottom, they allow use in
paramtric form rather than having to call separte subroutines for each
transformation.

(C) Alexander Heger (2021 -- 2023)
"""

import sys

import numpy as np
import quaternion as quat

from .rot import deg2rad, rad2deg
from .rot import q2f, w2f, f2w

#=======================================================================
# Euler and Tait-Bryan angles
#=======================================================================
# Here we follow the excellent article on Wikipedia (20211031)
# https://en.wikipedia.org/wiki/Euler_angles
# xyz will be use for intrinsic active rotations about x, y, z axes, in that order
# the resulting matrix is Ax Ay Az (apply z rotation first)
# the same matrix represents, in that order
# - extrinsic active rotations about z, y, x
# - intrinsic passive rotations about -z, -y, -x (Az.T Ay.T Ax.T)
# - extrinsic passive rotations about -x, -y, -z
#
# so, rotation about x, y, z, in that order, is the matrices
# - intrinsic active: Ax Ay Az
# - extrinsic active: Az Ay Ax
# - intrinsic passive: Az.T Ay.T Ax.T
# - extrinsic passive: Ax.T Ay.T Az.T
#
# (note A.T = inv(A), (A B C).T = C.T B.T A.T)
#
# (so the reverse of an intrinsic passive rotation is an extrinsic active rotation)
#
#=======================================================================

# https://stackoverflow.com/questions/30279065/how-to-get-the-euler-angles-from-the-rotation-vector-sensor-type-rotation-vecto
# https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

#=======================================================================
# converions of quaternions to rotation angles
#
# These are derived by comparing quaternion rotation matrix elements
# (general rotation matrix for a given quaternion) to those of the
# respective rotation matrix.  Easy.
#
#   R = [
#   [q0**2+q1**2-q2**2-q3**2, 2*(q1*q2-q0*q3)        , 2*(q0*q2+q1*q3)        ],
#   [2*(q1*q2+q0*q3)        , q0**2-q1**2+q2**2-q3**2, 2*(q2*q3-q0*q1)        ],
#   [2*(q1*q3-q0*q2)        , 2*(q0*q1+q2*q3)        , q0**2-q1**2-q2**3+q3**2],
#   ]

def f2xzy(f, deg = False):
    """
    Convert unit quaternion to TB angles XZY order. (Intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q1 + q2*q3), q0**2 - q1**2 + q2**2 - q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q0*q3 - q1*q2), 1), -1))
    a[..., 2] = np.arctan2(2*(q0*q2 + q1*q3), q0**2 + q1**2 - q2**2 - q3**2)
    if deg:
        return a * rad2deg
    return a


def f2xyz(f, deg = False):
    """
    Convert unit quaternion to TB angles XYZ order. (Intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q1 - q2*q3), q0**2 - q1**2 - q2**2 + q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q0*q2 + q1*q3), 1), -1))
    a[..., 2] = np.arctan2(2*(q0*q3 - q1*q2), q0**2 + q1**2 - q2**2 - q3**2)
    if deg:
        return a * rad2deg
    return a

def f2yxz(f, deg = False):
    """
    Convert unit quaternion to TB angles YXZ order. (Intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q2 + q1*q3), q0**2 - q1**2 - q2**2 + q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q0*q1 - q2*q3), 1), -1))
    a[..., 2] = np.arctan2(2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2)
    if deg:
        return a * rad2deg
    return a


def f2yzx(f, deg = False):
    """
    Convert unit quaternion to TB angles YZX order. (Intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q2 - q1*q3), q0**2 + q1**2 - q2**2 - q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q1*q2 + q0*q3), 1), -1))
    a[..., 2] = np.arctan2(2*(q0*q1 - q2*q3), q0**2 - q1**2 + q2**2 - q3**2)
    if deg:
        return a * rad2deg
    return a


def f2zyx(f, deg = False):
    """
    Convert unit quaternion to BT angles ZYX order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q3 + q1*q2), q0**2 + q1**2 - q2**2 - q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q0*q2 - q1*q3), 1), -1))
    a[..., 2] = np.arctan2(2*(q0*q1 + q2*q3), q0**2 - q1**2 - q2**2 + q3**2)
    if deg:
        return a * rad2deg
    return a


def f2zxy(f, deg = False):
    """
    Convert unit quaternion to BT angles ZXY order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(2*(q0*q3 - q1*q2), q0**2 - q1**2 + q2**2 - q3**2)
    a[..., 1] = np.arcsin(np.maximum(np.minimum(2*(q0*q1 + q2*q3), 1), -1))
    a[..., 2] = np.arctan2(2*(q0*q2 - q1*q3), q0**2 - q1**2 - q2**2 + q3**2)
    if deg:
        return a * rad2deg
    return a

# -----------------------------------------------------------------------

def f2xzx(f, deg = False):
    """
    Convert unit quaternion to Euler angles XZX order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q1*q3 - q0*q2, q1*q2 + q0*q3)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 + q1**2 - q2**2 - q3**2, 1), -1))
    a[..., 2] = np.arctan2(q0*q2 + q1*q3, q0*q3 - q2*q1)
    if deg:
        return a * rad2deg
    return a


def f2xyx(f, deg = False):
    """
    Convert unit quaternion to Euler angles XYX order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q1*q2 + q0*q3, q0*q2 - q1*q3)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 + q1**2 - q2**2 - q3**2, 1), -1))
    a[..., 2] = np.arctan2(q1*q2 - q0*q3, q0*q2 + q1*q3)
    if deg:
        return a * rad2deg
    return a


def f2yxy(f, deg = False):
    """
    Convert unit quaternion to Euler angles YXY order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q1*q2 - q0*q3, q0*q1 + q2*q3)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 - q1**2 + q2**2 - q3**2, 1), -1))
    a[..., 2] = np.arctan2(q1*q2 + q0*q3, q0*q1 - q2*q3)
    if deg:
        return a * rad2deg
    return a


def f2yzy(f, deg = False):
    """
    Convert unit quaternion to Euler angles YZY order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q0*q1 + q2*q3, q0*q3 - q1*q2)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 - q1**2 + q2**2 - q3**2, 1), -1))
    a[..., 2] = np.arctan2(q2*q3 - q0*q1, q1*q2 + q0*q3)
    if deg:
        return a * rad2deg
    return a


def f2zyz(f, deg = False):
    """
    Convert unit quaternion to Euler angles ZYZ order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q2*q3 - q0*q1, q0*q2 + q1*q3)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 - q1**2 - q2**2 + q3**2, 1), -1))
    a[..., 2] = np.arctan2(q0*q1 + q2*q3, q0*q2 - q1*q3)
    if deg:
        return a * rad2deg
    return a


def f2zxz(f, deg = False):
    """
    Convert unit quaternion to Euler angles ZXZ order (intrinsic, active)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    a = np.empty(f.shape[:-1] + (3,))
    a[..., 0] = np.arctan2(q0*q2 + q1*q3, q0*q1 - q2*q3)
    a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 - q1**2 - q2**2 + q3**2, 1), -1))
    a[..., 2] = np.arctan2(q1*q3 - q0*q2, q0*q1 + q2*q3)
    if deg:
        return a * rad2deg
    return a

# https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
#=======================================================================
# converions of angle to quaternions
#
#    These are derived by expressing rotations as quaternions, and the
#    multiplying these.  The unit rotation quaternions simply are
#
#    qx = [cos(phi/2), sin(phi/2), 0, 0]
#    qy = [cos(phi/2), 0, sin(phi/2), 0]
#    qz = [cos(phi/2), 0, 0, sin(phi/2)]
#
#    Easy.
#
#-----------------------------------------------------------------------
# Tait-Bryan angles
#
#    from sympy.algebras import Quaternion as Q
#    Q('cx','sx',0,0) * Q('cz',0,0,'sz') * Q('cy',0,'sy',0)
#    Q('cx','sx',0,0) * Q('cy',0,'sy',0) * Q('cz',0,0,'sz')
#    Q('cy',0,'sy',0) * Q('cx','sx',0,0) * Q('cz',0,0,'sz')
#    Q('cy',0,'sy',0) * Q('cz',0,0,'sz') * Q('cx','sx',0,0)
#    Q('cz',0,0,'sz') * Q('cy',0,'sy',0) * Q('cx','sx',0,0)
#    Q('cz',0,0,'sz') * Q('cx','sx',0,0) * Q('cy',0,'sy',0)
#

def xzy2f(e):
    """
    convert BT angles XZY to unit quaternion in float as q.shape + (4,)

    (intrinsic, active transform)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cx, cz, cy = np.cos(np.moveaxis(e, -1, 0))
    sx, sz, sy = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cx * cy * cz + sx * sy * sz
    f[..., 1] = cy * cz * sx - cx * sy * sz
    f[..., 2] = cx * cz * sy - cy * sx * sz
    f[..., 3] = cx * cy * sz + cz * sx * sy
    return f


def xyz2f(e):
    """
    convert BT angles XYZ to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cx, cy, cz = np.cos(np.moveaxis(e, -1, 0))
    sx, sy, sz = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cz * cy * cx - sz * sy * sx
    f[..., 1] = sz * sy * cx + cz * cy * sx
    f[..., 2] = cz * sy * cx - sz * cy * sx
    f[..., 3] = sz * cy * cx + cz * sy * sx
    return f


def yxz2f(e):
    """
    convert BT angles YXZ to unit quaternion in float as q.shape + (4,)

    (intrinsic, active transform)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cy, cx, cz = np.cos(np.moveaxis(e, -1, 0))
    sy, sx, sz = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cx * cy * cz + sx * sy * sz
    f[..., 1] = cx * sy * sz + cy * cz * sx
    f[..., 2] = cx * cz * sy - cy * sx * sz
    f[..., 3] = cx * cy * sz - cz * sx * sy
    return f


def yzx2f(e):
    """
    convert BT angles YZX to unit quaternion in float as q.shape + (4,)

    (intrinsic, active transform)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cy, cz, cx = np.cos(np.moveaxis(e, -1, 0))
    sy, sz, sx = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cx * cy * cz - sx * sy * sz
    f[..., 1] = cx * sy * sz + cy * cz * sx
    f[..., 2] = cx * cz * sy + cy * sx * sz
    f[..., 3] = cx * cy * sz - cz * sx * sy
    return f


def zyx2f(e):
    """
    convert BT angles ZYX to unit quaternion in float as q.shape + (4,)

    (intrinsic, active transform)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cz, cy, cx = np.cos(np.moveaxis(e, -1, 0))
    sz, sy, sx = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cz * cy * cx + sz * sy * sx
    f[..., 1] = cz * cy * sx - sz * sy * cx
    f[..., 2] = sz * cy * sx + cz * sy * cx
    f[..., 3] = sz * cy * cx - cz * sy * sx
    return f

def zxy2f(e):
    """
    convert BT angles ZXY to unit quaternion in float as q.shape + (4,)

    (intrinsic, active transform)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    cz, cx, cy = np.cos(np.moveaxis(e, -1, 0))
    sz, sx, sy = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = cx * cy * cz - sx * sy * sz
    f[..., 1] = cy * cz * sx - cx * sy * sz
    f[..., 2] = cx * cz * sy + cy * sx * sz
    f[..., 3] = cx * cy * sz + cz * sx * sy
    return f


#-----------------------------------------------------------------------
# Euler angles
#
#    from sympy.algebras import Quaternion as Q
#     Q('c1','s1',0,0) * Q('c2',0,0,'s2') * Q('c3','s3',0,0)
#     Q('c1','s1',0,0) * Q('c2',0,'s2',0) * Q('c3','s3',0,0)
#     Q('c1',0,'s1',0) * Q('c2','s2',0,0) * Q('c3',0,'s3',0)
#     Q('c1',0,'s1',0) * Q('c2',0,0,'s2') * Q('c3',0,'s3',0)
#     Q('c1',0,0,'s1') * Q('c2',0,'s2',0) * Q('c3',0,0,'s3')
#     Q('c1',0,0,'s1') * Q('c2','s2',0,0) * Q('c3',0,0,'s3')


def xzx2f(e):
    """
    convert Euler angles XZX to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c3 * c2 * s1 + c2 * c1 * s3
    f[..., 2] = c1 * s3 * s2 - c3 * s2 * s1
    f[..., 3] = c3 * c1 * s2 + s3 * s2 * s1
    return f


def xyx2f(e):
    """
    convert Euler angles XYX to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c3 * c2 * s1 + c2 * c1 * s3
    f[..., 2] = c3 * c1 * s2 + s3 * s2 * s1
    f[..., 3] = c3 * s2 * s1 - c1 * s3 * s2
    return f


def yxy2f(e):
    """
    convert Euler angles YXY to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c3 * c1 * s2 + s3 * s2 * s1
    f[..., 2] = c3 * c2 * s1 + c2 * c1 * s3
    f[..., 3] = c1 * s3 * s2 - c3 * s2 * s1
    return f


def yzy2f(e):
    """
    convert Euler angles YZY to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c3 * s2 * s1 - c1 * s3 * s2
    f[..., 2] = c3 * c2 * s1 + c2 * c1 * s3
    f[..., 3] = c3 * c1 * s2 + s3 * s2 * s1
    return f


def zyz2f(e):
    """
    convert Euler angles ZYZ to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c1 * s3 * s2 - c3 * s2 * s1
    f[..., 2] = c3 * c1 * s2 + s3 * s2 * s1
    f[..., 3] = c3 * c2 * s1 + c2 * c1 * s3
    return f


def zxz2f(e):
    """
    convert Euler angles ZXZ to unit quaternion

    (intrinsic, active)
    """
    e = np.asarray(e) * 0.5
    f = np.empty(e.shape[:-1] + (4,))
    c1, c2, c3 = np.cos(np.moveaxis(e, -1, 0))
    s1, s2, s3 = np.sin(np.moveaxis(e, -1, 0))
    f[..., 0] = c3 * c2 * c1 - c2 * s3 * s1
    f[..., 1] = c3 * c1 * s2 + s3 * s2 * s1
    f[..., 2] = c3 * s2 * s1 - c1 * s3 * s2
    f[..., 3] = c3 * c2 * s1 + c2 * c1 * s3
    return f


#-----------------------------------------------------------------------
# select interface/wrapper routines
#-----------------------------------------------------------------------

def w2xzy(w, deg=False):
    """
    convert rotation vector to BT angles XZY (active, intrinsic)
    """
    return f2xzy(w2f(w), deg=deg)

def w2xyz(w, deg=False):
    """
    convert rotation vector to BT angles XYZ (active, intrinsic)
    """
    return f2xyz(w2f(w), deg=deg)

def w2yxz(w, deg=False):
    """
    convert rotation vector to BT angles YXZ (active, intrinsic)
    """
    return f2yxz(w2f(w), deg=deg)

def w2yzx(w, deg=False):
    """
    convert rotation vector to BT angles YZX (active, intrinsic)
    """
    return f2yzx(w2f(w), deg=deg)

def w2zyx(w, deg=False):
    """
    convert rotation vector to BT angles ZYX (active, intrinsic)
    """
    return f2zyx(w2f(w), deg=deg)

def w2zxy(w, deg=False):
    """
    convert rotation vector to BT angles ZXY (active, intrinsic)
    """
    return f2zxy(w2f(w), deg=deg)

#.......................................................................

def w2xzx(w, deg=False):
    """
    convert rotation vector to Euler angles XZX (active, intrinsic)
    """
    return f2xzx(w2f(w), deg=deg)

def w2xyx(w, deg=False):
    """
    convert rotation vector to Euler angles XYX (active, intrinsic)
    """
    return f2xyx(w2f(w), deg=deg)

def w2yxy(w, deg=False):
    """
    convert rotation vector to Euler angles YXY (active, intrinsic)
    """
    return f2yxy(w2f(w), deg=deg)

def w2yzy(w, deg=False):
    """
    convert rotation vector to Euler angles YZY (active, intrinsic)
    """
    return f2yzy(w2f(w), deg=deg)

def w2zyz(w, deg=False):
    """
    convert rotation vector to Euler angles ZYZ (active, intrinsic)
    """
    return f2zyz(w2f(w), deg=deg)

def w2zxz(w, deg=False):
    """
    convert rotation vector to Euler angles ZXZ (active, intrinsic)
    """
    return f2zxz(w2f(w), deg=deg)

#-----------------------------------------------------------------------

def xzy2w(e):
    """
    Convert BT angles XZY (active, intrinsic) to rotation vector
    """
    return f2w(xzy2f(e))

def xyz2w(e):
    """
    Convert BT angles XYZ (active, intrinsic) to rotation vector
    """
    return f2w(xyz2f(e))

def yxz2w(e):
    """
    Convert BT angles YXZ (active, intrinsic) to rotation vector
    """
    return f2w(yxz2f(e))

def yzx2w(e):
    """
    Convert BT angles YZX (active, intrinsic) to rotation vector
    """
    return f2w(yzx2f(e))

def zyx2w(e):
    """
    Convert BT angles XYZ (active, intrinsic) to rotation vector
    """
    return f2w(zyx2f(e))

def zxy2w(e):
    """
    Convert BT angles ZXY (active, intrinsic) to rotation vector
    """
    return f2w(zxy2f(e))

#.......................................................................

def xzx2w(e):
    """
    Convert Euler angles XZX (active, intrinsic) to rotation vector
    """
    return f2w(xzx2f(e))

def xyx2w(e):
    """
    Convert Euler angles XYX (active, intrinsic) to rotation vector
    """
    return f2w(xyx2f(e))

def yxy2w(e):
    """
    Convert Euler angles YXY (active, intrinsic) to rotation vector
    """
    return f2w(yxy2f(e))

def yzy2w(e):
    """
    Convert Euler angles YZY (active, intrinsic) to rotation vector
    """
    return f2w(yzy2f(e))

def zyz2w(e):
    """
    Convert Euler angles ZYZ (active, intrinsic) to rotation vector
    """
    return f2w(zyz2f(e))

def zxz2w(e):
    """
    Convert Euler angles ZXZ (active, intrinsic) to rotation vector
    """
    return f2w(zxz2f(e))

#-----------------------------------------------------------------------

def xzy2q(e):
    """
    Convert BT angles XZY (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(xzy2f(e))

def xyz2q(e):
    """
    Convert BT angles XYZ (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(xyz2f(e))

def yxz2q(e):
    """
    Convert BT angles YXZ (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(yxz2f(e))

def yzx2q(e):
    """
    Convert BT angles YZX (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(yzx2f(e))

def zyx2q(e):
    """
    Convert BT angles ZYX to unit quaternion
    """
    return quat.from_float_array(zyx2f(e))

def zxy2q(e):
    """
    Convert BT angles ZXY to unit quaternion
    """
    return quat.from_float_array(zxy2f(e))

#.......................................................................

def xzx2q(e):
    """
    Convert Euler angles XZX (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(xzx2f(e))

def xyx2q(e):
    """
    Convert Euler angles XYX (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(xyx2f(e))

def yxy2q(e):
    """
    Convert Euler angles YXY (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(yxy2f(e))

def yzy2q(e):
    """
    Convert Euler angles YZY (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(yzy2f(e))

def zyz2q(e):
    """
    Convert Euler angles ZYZ (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(zyz2f(e))

def zxz2q(e):
    """
    Convert Euler angles ZXZ (active, intrinsic) to unit quaternion
    """
    return quat.from_float_array(zxz2f(e))

#-----------------------------------------------------------------------

def q2xzy(e):
    """
    convert unit quaternion to BT angles XZY (active, intrinsic)
    """
    return f2xzy(q2f(q))

def q2xyz(e):
    """
    convert unit quaternion to BT angles XYZ (active, intrinsic)
    """
    return f2xyz(q2f(q))

def q2yxz(e):
    """
    convert unit quaternion to BT angles YXZ (active, intrinsic)
    """
    return f2yxz(q2f(q))

def q2yzx(e):
    """
    convert unit quaternion to BT angles YZX (active, intrinsic)
    """
    return f2yzx(q2f(q))

def q2zyx(e):
    """
    convert unit quaternion to BT angles ZYX (active, intrinsic)
    """
    return f2zyx(q2f(q))

def q2zxy(e):
    """
    convert unit quaternion to BT angles ZXY (active, intrinsic)
    """
    return f2zxy(q2f(q))

#.......................................................................

def q2xzx(e):
    """
    convert unit quaternion to Euler angles XZX (active, intrinsic)
    """
    return f2xzx(q2f(q))

def q2xyx(e):
    """
    convert unit quaternion to Euler angles XYX (active, intrinsic)
    """
    return f2xyx(q2f(q))

def q2yxy(e):
    """
    convert unit quaternion to Euler angles YXY (active, intrinsic)
    """
    return f2yxy(q2f(q))

def q2yzy(e):
    """
    convert unit quaternion to Euler angles YZY (active, intrinsic)
    """
    return f2yzy(q2f(q))

def q2zyz(e):
    """
    convert unit quaternion to Euler angles ZYZ (active, intrinsic)
    """
    return f2zyz(q2f(q))

def q2zxz(e):
    """
    convert unit quaternion to Euler angles ZXZ (active, intrinsic)
    """
    return f2zxz(q2f(q))

#========================================================================
# https://en.wikipedia.org/wiki/Euler_angles
# XYZ -> angles 123
# intrinsic active X1 * Y2 * Z3
# (intrinisic rotation about X1 is logically first operation, but left-most matrix)

# -----------------------------------------------------------------------
# Tait-Bryan Angles
# -----------------------------------------------------------------------

# matrix A = [[a11 a12 a13], [a21 a22 a23], [a31 a32 a33]]


def xzy2A(e):
    """
    intrinsic active rotation about x, y, y, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [c2*c3,            -s2,    c2*s3           ],
        [s1*s3 + c1*c3*s2,  c1*c2, c1*s2*s3 - c3*s1],
        [c3*s1*s2 - c1*s3,  c2*s1, c1*c3 + s1*s2*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))

def xyz2A(e):
    """
    intrinsic active rotation about x, y, z, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [c2*c3,            -c2*s3,             s2   ],
        [c1*s3 + c3*s1*s2,  c1*c3 - s1*s2*s3, -c2*s1],
        [s1*s3 - c1*c3*s2,  c3*s1 + c1*s2*s3,  c1*c2],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def yxz2A(e):
    """
    intrinsic active rotation about y, x, z, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3,  c2*s1],
        [c2*s3,            c2*c3,            -s2   ],
        [c1*s2*s3 - c3*s1, c1*c3*s2 + s1*s3,  c1*c2],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def yzx2A(e):
    """
    intrinsic active rotation about y, z, x, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c2, s1*s3 - c1*c3*s2,  c3*s1 + c1*s2*s3],
        [ s2,    c2*c3,            -c2*s3   ],
        [-c2*s1, c1*s3 + c3*s1*s2,  c1*c3 - s1*s2*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def zyx2A(e):
    """
    intrinsic active rotation about z, y, x, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c2, c1*s2*s3 - c3*s1, s1*s3 + c1*c3*s2],
        [ c2*s1, c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3],
        [-s2,    c2*s3,            c2*c3           ],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def zxy2A(e):
    """
    intrinsic active rotation about z, x, y, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c3 - s1*s2*s3, -c2*s1, c1*s3 + c3*s1*s2],
        [ c3*s1 + c1*s2*s3,  c1*c2, s1*s3 - c1*c3*s2],
        [-c2*s3,             s2,    c2*c3           ],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))

# -----------------------------------------------------------------------

def A2xzy(A):
    """
    from matrix return vector with rotation angles about x, z, y axes

    intrinsic, active
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 2, 1], A[..., 1, 1])
    e[..., 1] = np.arcsin( -np.maximum(np.minimum(A[..., 0, 1], 1), -1))
    e[..., 2] = np.arctan2( A[..., 0, 2], A[..., 0, 0])
    return e

def A2xyz(A):
    """
    from matrix return vector with rotation angles about x, y, z axes

    intrinsic, active
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2(-A[..., 1, 2], A[..., 2, 2])
    e[..., 1] = np.arcsin(np.maximum(np.minimum(A[..., 0, 2], 1), -1))
    e[..., 2] = np.arctan2(-A[..., 0, 1], A[..., 0, 0])
    return e

def A2yxz(A):
    """
    from matrix return vector with rotation angles about y, x, z axes

    intrinsic, active
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 0, 2], A[..., 2, 2])
    e[..., 1] = np.arcsin(-np.maximum(np.minimum(A[..., 1, 2], 1), -1))
    e[..., 2] = np.arctan2( A[..., 1, 0], A[..., 1, 1])
    return e


def A2yzx(A):
    """
    from matrix return vector with rotation angles about y, z, x axes

    intrinsic, active
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2(-A[..., 2, 0], A[..., 0, 0])
    e[..., 1] = np.arcsin( np.maximum(np.minimum(A[..., 1, 0], 1), -1))
    e[..., 2] = np.arctan2(-A[..., 1, 2], A[..., 1, 1])
    return e


def A2zyx(A):
    """
    from matrix return vector with rotation angles about z, y, x axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 1, 0], A[..., 0, 0])
    e[..., 1] = np.arcsin(-np.maximum(np.minimum(A[..., 2, 0], 1), -1))
    e[..., 2] = np.arctan2( A[..., 2, 1], A[..., 2, 2])
    return e


def A2zxy(A):
    """
    from matrix return vector with rotation angles about z, x, y axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2(-A[..., 0, 1], A[..., 1, 1])
    e[..., 1] = np.arcsin( np.maximum(np.minimum(A[..., 2, 1], 1), -1))
    e[..., 2] = np.arctan2(-A[..., 2, 0], A[..., 2, 2])
    return e

# -----------------------------------------------------------------------
# Euler Angles
# -----------------------------------------------------------------------

def xzx2A(e):
    """
    intrinsic active rotation about x, z, x, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c2,   -c3*s2,             s2*s3           ],
        [ c1*s2, c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3],
        [ s1*s2, c1*s3 + c2*c3*s1,  c1*c3 - c2*s1*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def xyx2A(e):
    """
    intrinsic active rotation about x, y, x, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c2,    s2*s3,             c3*s2           ],
        [ s1*s2, c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1],
        [-c1*s2, c3*s1 + c1*c2*s3,  c1*c2*c3 - s1*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def yxy2A(e):
    """
    intrinsic active rotation about y, x, y, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c3 - c2*s1*s3,  s1*s2,  c1*s3 + c2*c3*s1],
        [ s2*s3,             c2,    -c3*s2           ],
        [-c3*s1 - c1*c2*s3,  c1*s2,  c1*c2*c3 - s1*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def yzy2A(e):
    """
    intrinsic active rotation about y, z, y, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c2*c3 - s1*s3, -c1*s2, c3*s1 + c1*c2*s3],
        [ c3*s2,             c2,    s2*s3           ],
        [-c1*s3 - c2*c3*s1,  s1*s2, c1*c3 - c2*s1*s3],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def zyz2A(e):
    """
    intrinsic active rotation about z, y, z, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, c1*s2],
        [ c1*s3 + c2*c3*s1,  c1*c3 - c2*s1*s3, s1*s2],
        [-c3*s2,             s2*s3,            c2   ],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))


def zxz2A(e):
    """
    intrinsic active rotation about z, x, z, in that order
    """
    e = np.moveaxis(np.asarray(e), -1, 0)
    c1, c2, c3 = np.cos(e)
    s1, s2, s3 = np.sin(e)
    A = np.array([
        [ c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1,  s1*s2],
        [ c3*s1 + c1*c2*s3,  c1*c2*c3 - s1*s3, -c1*s2],
        [ s2*s3,             c3*s2,             c2   ],
        ])
    return np.moveaxis(A, (0,1), (-2,-1))

# -----------------------------------------------------------------------

def A2xzx(A):
    """
    from matrix return vector with rotation angles about x, z, x axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 2, 0],  A[..., 1, 0])
    e[..., 1] = np.arccos( np.maximum(np.minimum(A[..., 0, 0], 1), -1))
    e[..., 2] = np.arctan2( A[..., 0, 2], -A[..., 0, 1])
    return e


def A2xyx(A):
    """
    from matrix return vector with rotation angles about x, y, x axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 1, 0], -A[..., 2, 0])
    e[..., 1] = np.arccos( np.maximum(np.minimum(A[..., 0, 0], 1), -1))
    e[..., 2] = np.arctan2( A[..., 0, 1],  A[..., 0, 2])
    return e


def A2yxy(A):
    """
    from matrix return vector with rotation angles about y, x, y axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 0, 1],  A[..., 2, 1])
    e[..., 1] = np.arccos( np.maximum(np.minimum(A[..., 1, 1], 1), -1))
    e[..., 2] = np.arctan2( A[..., 1, 0], -A[..., 1, 2])
    return e


def A2yzy(A):
    """
    from matrix return vector with rotation angles about y, z, y axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 2, 1], -A[..., 0, 1])
    e[..., 1] = np.arccos( np.maximum(np.minimum(A[..., 1, 1], 1), -1))
    e[..., 2] = np.arctan2( A[..., 1, 2],  A[..., 1, 0])
    return e


def A2zyz(A):
    """
    from matrix return vector with rotation angles about z, y, z axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 1, 2],  A[..., 0, 2])
    e[..., 1] = np.arccos( np.maximum(np.minimum(A[..., 2, 2], 1), -1))
    e[..., 2] = np.arctan2( A[..., 2, 1], -A[..., 2, 0])
    return e


def A2zxz(A):
    """
    from matrix return vector with rotation angles about z, x, z axes
    """
    A = np.asarray(A)
    e = np.ndarray(A.shape[:-2] + (3,))
    e[..., 0] = np.arctan2( A[..., 0, 2], -A[..., 1, 2])
    e[..., 1] = np.arccos(np.maximum(np.minimum(A[..., 2, 2], 1), -1))
    e[..., 2] = np.arctan2( A[..., 2, 0], A[..., 2, 1])
    return e

# -----------------------------------------------------------------------
# generic routines
# -----------------------------------------------------------------------

def e2f(e, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert Euler and Tait-Bryan angles to unit quaternions

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    e = np.asarray(e)
    if deg:
        e = e * deg2rad
    if intrinsic ^ active:
        order = order[::-1]
        e = e[..., ::-1]
    if not active:
        e = -e
    return getattr(sys.modules[globals()['__name__']], f'{order}2f')(e)


def f2e(f, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert unit quaternions to Euler and Tait-Bryan angles

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    f = np.asarray(f)
    if intrinsic ^ active:
        order = order[::-1]
    e = getattr(sys.modules[globals()['__name__']], f'f2{order}')(f)
    if intrinsic ^ active:
        e = e[..., ::-1]
    if not active:
        e = -e
    if deg:
        e = e * rad2deg
    return e

# -----------------------------------------------------------------------

def e2A(e, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert Euler and Tait-Bryan angles to rotation matrix

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    e = np.asarray(e)
    if deg:
        e = e * deg2rad
    if intrinsic ^ active:
        order = order[::-1]
        e = e[..., ::-1]
    if not active:
        e = -e
    return getattr(sys.modules[globals()['__name__']], f'{order}2A')(e)


def A2e(A, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert rotation matrix to Euler and Tait-Bryan angles

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    A = np.asarray(A)
    if intrinsic ^ active:
        order = order[::-1]
    e = getattr(sys.modules[globals()['__name__']], f'A2{order}')(A)
    if intrinsic ^ active:
        e = e[..., ::-1]
    if not active:
        e = -e
    if deg:
        e = e * rad2deg
    return e

# -----------------------------------------------------------------------

def e2w(e, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert Euler and Tait-Bryan angles to rotation vector

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    e = np.asarray(e)
    if deg:
        e = e * deg2rad
    if intrinsic ^ active:
        order = order[::-1]
        e = e[..., ::-1]
    if not active:
        e = -e
    return getattr(sys.modules[globals()['__name__']], f'{order}2w')(e)


def w2e(w, order='zxz', intrinsic=True, active=True, deg=False):
    """
    convert rotation vectors to Euler and Tait-Bryan angles

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    w = np.asarray(w)
    if intrinsic ^ active:
        order = order[::-1]
    e = getattr(sys.modules[globals()['__name__']], f'w2{order}')(w)
    if intrinsic ^ active:
        e = e[..., ::-1]
    if not active:
        e = -e
    if deg:
        e = e * rad2deg
    return e

# -----------------------------------------------------------------------

def e2q(e, order='zxz', intrinsic=True, active=True, deg=False):
    """
    Convert Euler and Tait-Bryan angles to unit quaternions

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    e = np.asarray(e)
    if deg:
        e = e * deg2rad
    if intrinsic ^ active:
        order = order[::-1]
        e = e[..., ::-1]
    if not active:
        e = -e
    return getattr(sys.modules[globals()['__name__']], f'{order}2q')(e)


def q2e(q, order='zxz', intrinsic=True, active=True, deg=False):
    """
    convert unit quaternions to Euler and Tait-Bryan angles

    (as far as defined)
    """
    assert isinstance(order, str)
    assert len(order) == 3
    q = np.asarray(q)
    if intrinsic ^ active:
        order = order[::-1]
    e = getattr(sys.modules[globals()['__name__']], f'q2{order}')(q)
    if intrinsic ^ active:
        e = e[..., ::-1]
    if not active:
        e = -e
    if deg:
        e = e * rad2deg
    return e


# ======================================================================
# very select derivatives
# ======================================================================

def f2zxzdf(f, deg = False):
    """
    Derivative of conversion of unit quaternion to Euler angles ZXZ order
    with respect to quaternion (3x4 Jacobian matrix)
    (intrinsic, active)

    use arctan2(y,x)
    d/dx arctan2 = -y/(x**2 + y**2)
    d/dy arctan2 =  x/(x**2 + y**2)

    d/dx arccos(x) = -1/sqrt(1 - x**2)
    """
    q0, q1, q2, q3 = np.moveaxis(f, -1, 0)
    q02, q12, q22, q32 = np.moveaxis(f, -1, 0)**2
    da = np.empty(f.shape[:-1] + (3,4,))

    # a[..., 0] = np.arctan2(q0*q2 + q1*q3, q0*q1 - q2*q3)
    y0 = q1*q3 + q0*q2
    x0 = q0*q1 - q2*q3
    xy2i = 1 / ((q02 + q32)*(q12 + q22))
    t0x = -y0 * xy2i
    t0y =  x0 * xy2i
    da[..., 0, 0] = t0y * q2 + t0x * q1
    da[..., 0, 1] = t0y * q3 + t0x * q0
    da[..., 0, 2] = t0y * q0 - t0x * q3
    da[..., 0, 3] = t0y * q1 - t0x * q2

    # a[..., 1] = np.arccos(np.maximum(np.minimum(q0**2 - q1**2 - q2**2 + q3**2, 1), -1))
    x1 = q02 - q12 - q22 + q32
    t1x = -2 / np.sqrt(1 - x1**2)
    da[..., 1, 0] =   t1x * q0
    da[..., 1, 1] = - t1x * q1
    da[..., 1, 2] = - t1x * q2
    da[..., 1, 3] =   t1x * q3

    # a[..., 2] = np.arctan2(q1*q3 - q0*q2, q0*q1 + q2*q3)
    y2 = q1*q3 - q0*q2
    x2 = q0*q1 + q2*q3
    # xy2i = 1 / ((q02 + q32)*(q12 + q22)) # duplicate
    t2x = -y2 * xy2i
    t2y =  x2 * xy2i
    da[..., 2, 0] = t2x * q1 - t2y * q2
    da[..., 2, 1] = t2x * q0 + t2y * q3
    da[..., 2, 2] = t2x * q3 - t2y * q0
    da[..., 2, 3] = t2x * q2 + t2y * q1
    if deg:
        return da * rad2deg
    return da