# Path: sensbiotk/driver/fox_dongle.py
# class FoxDongle():
#     def __init__(self):
#     def init_dongle(self, funcname = None):
#     def close_dongle(self):
#     def is_running(self):
#     def line(self):
#     def read(self):
#     def unstack_read(self):
#     def read_all(self):
#     def store(self):
# def test_acq_callback(obj):
# def test1():
# def test2():
# 
# Path: sensbiotk/transforms3d/eulerangles.py
# def quat2euler(q):
#     ''' Return Euler angles corresponding to quaternion `q`
# 
#     Parameters
#     ----------
#     q : 4 element sequence
#        w, x, y, z of quaternion
# 
#     Returns
#     -------
#     z : scalar
#        Rotation angle in radians around z-axis (performed first)
#     y : scalar
#        Rotation angle in radians around y-axis
#     x : scalar
#        Rotation angle in radians around x-axis (performed last)
# 
#     Notes
#     -----
#     It's possible to reduce the amount of calculation a little, by
#     combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
#     the reduction in computation is small, and the code repetition is
#     large.
#     '''
#     # delayed import to avoid cyclic dependencies
#     import sensbiotk.transforms3d.quaternions as nq
#     return mat2euler(nq.quat2mat(q))
# 
# 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_mag.py
# SEP = ";"
# def unit_sphere_fit(data):
# def compute(data):


from sensbiotk.driver import fox_dongle as fdongle
from sensbiotk.transforms3d.eulerangles import quat2euler
from sensbiotk.transforms3d import quaternions as nq
from visual import *
from sensbiotk.calib import calib_mag
import time
import numpy as np
import logging
import sensbiotk.algorithms.martin_ahrs as martin

                        data_calib = np.zeros((4000,11))
                        for i in range(1,4000):
                            data_calib[i] = foxdongle.read().reshape(1, 11)
                            time.sleep(0.005)
                        offset, scale = calib_mag.compute(data_calib[:,5:8])
                        print scale
                        print offset
                        print 'Calib magneto over'
                        print 'Initializing the observer...'
                        calib_mag_bool = True
                    if not init_observer and calib_mag_bool: # record data for initializing the observer
                        print 'Please stay without moving in the initial position (as VPython)'
                        time.sleep(4)
                        data_obs = np.zeros((1000, 11))
                        for i in range(0,1000):
                            data = foxdongle.read().reshape(1, 11)
                            data[0, 5:8] = np.transpose(np.dot(scale,np.transpose((data[0, 5:8]-np.transpose(offset)))))
                            data_obs[i,:] = data[:]
                            time.sleep(0.005)
                            print('.'),
                        rt.init_observer(np.mean(data_obs[:,2:12],0))
                        init_observer = True
                        print 'Start animation...'
                        rt.text.height = 0
                    if init_observer and not init_frame:
                        for i in range(0, 1000): # waits that the algorithm converges
                            data = foxdongle.read().reshape(1, 11)
                            data[0, 5:8] = np.transpose(np.dot(scale,np.transpose((data[0, 5:8]-np.transpose(offset)))))
                            rt.update(data)
                            time.sleep(0.005)

# Path: sensbiotk/driver/fox_dongle.py
# class FoxDongle():
#     def __init__(self):
#     def init_dongle(self, funcname = None):
#     def close_dongle(self):
#     def is_running(self):
#     def line(self):
#     def read(self):
#     def unstack_read(self):
#     def read_all(self):
#     def store(self):
# def test_acq_callback(obj):
# def test1():
# def test2():
# 
# Path: sensbiotk/transforms3d/eulerangles.py
# def quat2euler(q):
#     ''' Return Euler angles corresponding to quaternion `q`
# 
#     Parameters
#     ----------
#     q : 4 element sequence
#        w, x, y, z of quaternion
# 
#     Returns
#     -------
#     z : scalar
#        Rotation angle in radians around z-axis (performed first)
#     y : scalar
#        Rotation angle in radians around y-axis
#     x : scalar
#        Rotation angle in radians around x-axis (performed last)
# 
#     Notes
#     -----
#     It's possible to reduce the amount of calculation a little, by
#     combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
#     the reduction in computation is small, and the code repetition is
#     large.
#     '''
#     # delayed import to avoid cyclic dependencies
#     import sensbiotk.transforms3d.quaternions as nq
#     return mat2euler(nq.quat2mat(q))
# 
# 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_mag.py
# SEP = ";"
# def unit_sphere_fit(data):
# def compute(data):


# Path: examples/real_time_ahrs/real_time_martin_ahrs_visu.py
from sensbiotk.driver import fox_dongle as fdongle
from sensbiotk.transforms3d.eulerangles import quat2euler
from sensbiotk.transforms3d import quaternions as nq
from visual import *
from sensbiotk.calib import calib_mag
import time
import numpy as np
import logging
import sensbiotk.algorithms.martin_ahrs as martin

                        data_calib = np.zeros((4000,11))
                        for i in range(1,4000):
                            data_calib[i] = foxdongle.read().reshape(1, 11)
                            time.sleep(0.005)
                        offset, scale = calib_mag.compute(data_calib[:,5:8])
                        print scale
                        print offset
                        print 'Calib magneto over'
                        print 'Initializing the observer...'
                        calib_mag_bool = True
                    if not init_observer and calib_mag_bool: # record data for initializing the observer
                        print 'Please stay without moving in the initial position (as VPython)'
                        time.sleep(4)
                        data_obs = np.zeros((1000, 11))
                        for i in range(0,1000):
                            data = foxdongle.read().reshape(1, 11)
                            data[0, 5:8] = np.transpose(np.dot(scale,np.transpose((data[0, 5:8]-np.transpose(offset)))))
                            data_obs[i,:] = data[:]
                            time.sleep(0.005)
                            print('.'),
                        rt.init_observer(np.mean(data_obs[:,2:12],0))
                        init_observer = True
                        print 'Start animation...'
                        rt.text.height = 0
                    if init_observer and not init_frame:
                        for i in range(0, 1000): # waits that the algorithm converges
                            data = foxdongle.read().reshape(1, 11)
                            data[0, 5:8] = np.transpose(np.dot(scale,np.transpose((data[0, 5:8]-np.transpose(offset)))))
                            rt.update(data)
                            time.sleep(0.005)

                        rt.quat_offset = nq.conjugate(rt.quaternion)
