# 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 math
from sensbiotk.transforms3d import quaternions as nq
#    h = [1, 1, 1, 1]
    b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])
    # Gradient decent algorithm corrective step
    F = np.array([[2*(q[1]*q[3] - q[0]*q[2]) - accelero[0]],
           [2*(q[0]*q[1] + q[2]*q[3]) - accelero[1]],
          [2*(0.5 - q[1]**2 - q[2]**2) - accelero[2]],
          [2*b[1]*(0.5 - q[2]**2 - q[3]**2)
              + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - magneto[0]],
           [2*b[1]*(q[1]*q[2] - q[0]*q[3])
              + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - magneto[1]],
           [2*b[1]*(q[0]*q[2] + q[1]*q[3])
              + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - magneto[2]]])

    J = np.array([[-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
         [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
         [0, -4*q[1], -4*q[2], 0],
         [-2*b[3]*q[2], 2*b[3]*q[3],
           4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
          [-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0],
            2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
          [2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2],
           2*b[1]*q[1]]])


    step = (np.dot(np.transpose(J), F))
    # normalise step magnitude
    step = step / norm(step)

    # Compute rate of change of quaternion
    q_dot = np.dot(

# 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/madgwick_ahrs.py
import numpy as np
import math
from sensbiotk.transforms3d import quaternions as nq

#    h = [1, 1, 1, 1]
    b = np.array([0, np.linalg.norm([h[1], h[2]]), 0, h[3]])
    # Gradient decent algorithm corrective step
    F = np.array([[2*(q[1]*q[3] - q[0]*q[2]) - accelero[0]],
           [2*(q[0]*q[1] + q[2]*q[3]) - accelero[1]],
          [2*(0.5 - q[1]**2 - q[2]**2) - accelero[2]],
          [2*b[1]*(0.5 - q[2]**2 - q[3]**2)
              + 2*b[3]*(q[1]*q[3] - q[0]*q[2]) - magneto[0]],
           [2*b[1]*(q[1]*q[2] - q[0]*q[3])
              + 2*b[3]*(q[0]*q[1] + q[2]*q[3]) - magneto[1]],
           [2*b[1]*(q[0]*q[2] + q[1]*q[3])
              + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - magneto[2]]])

    J = np.array([[-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
         [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
         [0, -4*q[1], -4*q[2], 0],
         [-2*b[3]*q[2], 2*b[3]*q[3],
           4*b[1]*q[2]-2*b[3]*q[0], -4*b[1]*q[3]+2*b[3]*q[1]],
          [-2*b[1]*q[3]+2*b[3]*q[1], 2*b[1]*q[2]+2*b[3]*q[0],
            2*b[1]*q[1]+2*b[3]*q[3], -2*b[1]*q[0]+2*b[3]*q[2]],
          [2*b[1]*q[2], 2*b[1]*q[3]-4*b[3]*q[1], 2*b[1]*q[0]-4*b[3]*q[2],
           2*b[1]*q[1]]])


    step = (np.dot(np.transpose(J), F))
    # normalise step magnitude
    step = step / norm(step)

    # Compute rate of change of quaternion
    q_dot = np.dot(

    0.5, nq.mult(q, (0, gyro[0], gyro[1], gyro[2]))) - \