# 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


class obj:
    eInt = 0

def norm(x):
#    print "norm:", x
    tmpnorm = 0
    for i in range(0, len(x)):
        tmpnorm = tmpnorm + x[i]**2
    return math.sqrt(tmpnorm)

def update(q, z, fs=200):

    accelero = z[0:3]
    magneto = z[3:6]
    gyro = z[6:9]

    sample_period = 1./fs
    Kp = 0.01 # algorithm proportional gain
    Ki = 0 # algorithm integral gain

    # Normalise accelerometer measurement
    norm_accelero = norm(accelero)
    if  norm_accelero != 0:
        accelero = accelero / norm_accelero
    # Normalise magnetometer measurement
    norm_magneto = norm(magneto)
    if  norm_magneto != 0:
        magneto = magneto / norm_magneto
    # Reference direction of Earth's magnetic field

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


class obj:
    eInt = 0

def norm(x):
#    print "norm:", x
    tmpnorm = 0
    for i in range(0, len(x)):
        tmpnorm = tmpnorm + x[i]**2
    return math.sqrt(tmpnorm)

def update(q, z, fs=200):

    accelero = z[0:3]
    magneto = z[3:6]
    gyro = z[6:9]

    sample_period = 1./fs
    Kp = 0.01 # algorithm proportional gain
    Ki = 0 # algorithm integral gain

    # Normalise accelerometer measurement
    norm_accelero = norm(accelero)
    if  norm_accelero != 0:
        accelero = accelero / norm_accelero
    # Normalise magnetometer measurement
    norm_magneto = norm(magneto)
    if  norm_magneto != 0:
        magneto = magneto / norm_magneto
    # Reference direction of Earth's magnetic field

    h = nq.mult(q, nq.mult(np.concatenate(([0], magneto)), nq.conjugate(q)))
