# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):


import numpy as np
from sensbiotk.transforms3d import quaternions as nq


# disabling pylint errors 'E1101' no-member, false positive from pylint
# disabling pylint errors 'C0103' invalid variable name, for variables : a,b
# pylint:disable=I0011,E1101,C0103

def compute(q_0, q_1, q_offset=[1, 0, 0, 0]):

    """ Computes rotation angle between two quaternions

    Parameters :
    ------------
    q_0  : numpy array of float
                 the first quaternion (w, x, y, z)
    q_1 : numpy array of float
                 the second quaternion (w, x, y, z)
    q_offset : numpy array of float
                 the initial quaternion corresponding to the offset
                 between q_0 and q_1 at an initial position
                 (can be generated using sensbiotk.calib.calib_geom)

    Returns
    -------
     angle : float
             the angle of rotation between q_0 and q_1 (rad)
     rot_axis : numpy array of float
             the axis of rotation

    """

    q_corr = nq.mult(

# Path: sensbiotk/transforms3d/quaternions.py
# MAX_FLOAT = np.maximum_sctype(np.float)
# FLOAT_EPS = np.finfo(np.float).eps
#     X = x*s
#     Y = y*s
#     Z = z*s
#     K = np.array([
#         [Qxx - Qyy - Qzz, 0,               0,               0              ],
#         [Qyx + Qxy,       Qyy - Qxx - Qzz, 0,               0              ],
#         [Qzx + Qxz,       Qzy + Qyz,       Qzz - Qxx - Qyy, 0              ],
#         [Qyz - Qzy,       Qzx - Qxz,       Qxy - Qyx,       Qxx + Qyy + Qzz]]
#         ) / 3.0
#     c = math.cos(theta); s = math.sin(theta); C = 1-c
# def fillpositive(xyz, w2_thresh=None):
# def quat2mat(q):
# def mat2quat(M):
# def mult(q1, q2):
# def conjugate(q):
# def norm(q):
# def norm2(q):
# def normalize(q):
# def isunit(q):
# def inverse(q):
# def eye():
# def rotate_vector(v, q):
# def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8):
# def angle_axis2quat(theta, vector, is_normalized=False):
# def angle_axis2mat(theta, vector, is_normalized=False):
# def quat2angle_axis(quat, identity_thresh=None):


# Path: sensbiotk/algorithms/goniometer.py
import numpy as np
from sensbiotk.transforms3d import quaternions as nq


# disabling pylint errors 'E1101' no-member, false positive from pylint
# disabling pylint errors 'C0103' invalid variable name, for variables : a,b
# pylint:disable=I0011,E1101,C0103

def compute(q_0, q_1, q_offset=[1, 0, 0, 0]):

    """ Computes rotation angle between two quaternions

    Parameters :
    ------------
    q_0  : numpy array of float
                 the first quaternion (w, x, y, z)
    q_1 : numpy array of float
                 the second quaternion (w, x, y, z)
    q_offset : numpy array of float
                 the initial quaternion corresponding to the offset
                 between q_0 and q_1 at an initial position
                 (can be generated using sensbiotk.calib.calib_geom)

    Returns
    -------
     angle : float
             the angle of rotation between q_0 and q_1 (rad)
     rot_axis : numpy array of float
             the axis of rotation

    """

    q_corr = nq.mult(

        nq.mult(np.transpose(nq.conjugate(q_0)),
