# 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
import sensbiotk.algorithms.madgwick_ahrs as madgwick
import sensbiotk.algorithms.martin_ahrs as martin
from sensbiotk.transforms3d import quaternions as nq

    """ Compute an "offset" quaternion between two supposed aligned quaternions
    
    Parameters :
    ------------
    data_imu0  : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]
    data_imu1 : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]

    Returns
    -------
    q_offset : quaternion [w, x, y, z]
                 the offset quaternion between IMU0 and IMU1
    """

    # Averages the data on the reference position
    data_ref_imu0 = np.mean(data_imu0[:,0:10],0)
    data_ref_imu1 = np.mean(data_imu1[:,0:10],0)

    # madgwick calculation
#    q_init=[1, 0, 0, 0]
#    q0 = madgwick.update(q_init, data_ref_imu0)
#    q1 = madgwick.update(q_init, data_ref_imu1)   

    # Uses martin salaun for calculating the two quaternions
    init0 = martin.martin_ahrs()
    init1 = martin.martin_ahrs()
    q0 = init0.init_observer(data_ref_imu0)
    q1 = init1.init_observer(data_ref_imu1)        
    

# 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/calib/calib_geom.py
import numpy as np
import sensbiotk.algorithms.madgwick_ahrs as madgwick
import sensbiotk.algorithms.martin_ahrs as martin
from sensbiotk.transforms3d import quaternions as nq

    """ Compute an "offset" quaternion between two supposed aligned quaternions
    
    Parameters :
    ------------
    data_imu0  : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]
    data_imu1 : numpy array of float
                 the IMU column stacked reference position data [ACC MAG GYR]

    Returns
    -------
    q_offset : quaternion [w, x, y, z]
                 the offset quaternion between IMU0 and IMU1
    """

    # Averages the data on the reference position
    data_ref_imu0 = np.mean(data_imu0[:,0:10],0)
    data_ref_imu1 = np.mean(data_imu1[:,0:10],0)

    # madgwick calculation
#    q_init=[1, 0, 0, 0]
#    q0 = madgwick.update(q_init, data_ref_imu0)
#    q1 = madgwick.update(q_init, data_ref_imu1)   

    # Uses martin salaun for calculating the two quaternions
    init0 = martin.martin_ahrs()
    init1 = martin.martin_ahrs()
    q0 = init0.init_observer(data_ref_imu0)
    q1 = init1.init_observer(data_ref_imu1)        
    

    q_offset=nq.mult(np.transpose(nq.conjugate(q0)),np.transpose(q1)) # q_offset = conj(q0) x q1
