# Path: sensbiotk/io/iofox.py
# def load_foxcsvfile(filename):
#     """ Load IMU HikoB Fox Node data from a CSV file version 2\n
#     id    t    dt    ax    ay    az    mx    my    mz    gx    gy    gz\n
#     columns are separated by a tab
# 
#     Parameters
#     ----------
#     filename : str
#     Name of the CSV file containing inertial data
# 
#     Return
#     -------
#     time : numpy.array
#     accx : numpy.array
#     accy : numpy.array
#     accz : numpy.array
#     magx : numpy.array
#     magy : numpy.array
#     magz : numpy.array
#     gyrx : numpy.array
#     gyry : numpy.array
#     gyrz : numpy.array
#     """
# 
#     data = np.loadtxt(filename, delimiter='\t')
# 
#     time = data[:, 1]
#     accx = data[:, 3]
#     accy = data[:, 4]
#     accz = data[:, 5]
#     magx = data[:, 6]
#     magy = data[:, 7]
#     magz = data[:, 8]
#     gyrx = data[:, 9]
#     gyry = data[:, 10]
#     gyrz = data[:, 11]
# 
#     return [time, accx, accy, accz, magx, magy, magz, gyrx, gyry, gyrz]
# 
# 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))


import numpy as np 
import matplotlib.pyplot as plt
import sensbiotk.algorithms.madgwick_ahrs as madgwick
import sensbiotk.calib.calib as calib
import sensbiotk.transforms3d.quaternions as nq
import scipy.io as sio
from sensbiotk.io.iofox import load_foxcsvfile
from sensbiotk.transforms3d.eulerangles import quat2euler
from visual import *

    scale_mag = params_mag[1:4,:]
    bias_mag = params_mag[0,:]
    scale_gyr = params_gyr[1:4,:]
    bias_gyr = params_gyr[0,:]       
    
    acc_imu = np.column_stack([accx, accy, accz])
    mag_imu = np.column_stack([mx, my, mz])
    gyr_imu = np.column_stack([gyrx, gyry, gyrz])
    
    quaternion = np.zeros((len(acc_imu),4))
    euler = np.zeros((len(acc_imu),3))
    
    quaternion[0,:]=[1, 0, 0, 0]
        
    for i in range(0,len(acc_imu)-1):
        acc_imu[i,:]= np.transpose(np.dot(scale_acc,np.transpose((acc_imu[i,:]-np.transpose(bias_acc)))))
        mag_imu[i,:]=  np.transpose(np.dot(scale_mag,np.transpose((mag_imu[i,:]-np.transpose(bias_mag)))))
        gyr_imu[i,:]=  np.transpose(np.dot(scale_gyr,np.transpose((gyr_imu[i,:]-np.transpose(bias_gyr)))))
        quaternion[i+1]= madgwick.update(quaternion[i],np.hstack([acc_imu[i,:],mag_imu[i,:], gyr_imu[i,:]]))
        euler[i]=quat2euler(quaternion[i+1])
    
    #######################
    # Plots
    ####################### 
    dataVicon = np.loadtxt('data/RIGHT_FOOT.txt',skiprows=4)
    time_vicon = dataVicon[:,0]
    euler_vicon= np.zeros((len(time_vicon),3))
    quat_offset = np.hstack([dataVicon[0,7], dataVicon[0,4], dataVicon[0,5], dataVicon[0,6]])
    for i in range(1, len(time_vicon)-1):
        quat_vicon = np.hstack([dataVicon[i,7], dataVicon[i,4], dataVicon[i,5], dataVicon[i,6]])

# Path: sensbiotk/io/iofox.py
# def load_foxcsvfile(filename):
#     """ Load IMU HikoB Fox Node data from a CSV file version 2\n
#     id    t    dt    ax    ay    az    mx    my    mz    gx    gy    gz\n
#     columns are separated by a tab
# 
#     Parameters
#     ----------
#     filename : str
#     Name of the CSV file containing inertial data
# 
#     Return
#     -------
#     time : numpy.array
#     accx : numpy.array
#     accy : numpy.array
#     accz : numpy.array
#     magx : numpy.array
#     magy : numpy.array
#     magz : numpy.array
#     gyrx : numpy.array
#     gyry : numpy.array
#     gyrz : numpy.array
#     """
# 
#     data = np.loadtxt(filename, delimiter='\t')
# 
#     time = data[:, 1]
#     accx = data[:, 3]
#     accy = data[:, 4]
#     accz = data[:, 5]
#     magx = data[:, 6]
#     magy = data[:, 7]
#     magz = data[:, 8]
#     gyrx = data[:, 9]
#     gyry = data[:, 10]
#     gyrz = data[:, 11]
# 
#     return [time, accx, accy, accz, magx, magy, magz, gyrx, gyry, gyrz]
# 
# 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: examples/hemi_gait_vicon/simple_rotation_madgwick.py
import numpy as np 
import matplotlib.pyplot as plt
import sensbiotk.algorithms.madgwick_ahrs as madgwick
import sensbiotk.calib.calib as calib
import sensbiotk.transforms3d.quaternions as nq
import scipy.io as sio
from sensbiotk.io.iofox import load_foxcsvfile
from sensbiotk.transforms3d.eulerangles import quat2euler
from visual import *

    scale_mag = params_mag[1:4,:]
    bias_mag = params_mag[0,:]
    scale_gyr = params_gyr[1:4,:]
    bias_gyr = params_gyr[0,:]       
    
    acc_imu = np.column_stack([accx, accy, accz])
    mag_imu = np.column_stack([mx, my, mz])
    gyr_imu = np.column_stack([gyrx, gyry, gyrz])
    
    quaternion = np.zeros((len(acc_imu),4))
    euler = np.zeros((len(acc_imu),3))
    
    quaternion[0,:]=[1, 0, 0, 0]
        
    for i in range(0,len(acc_imu)-1):
        acc_imu[i,:]= np.transpose(np.dot(scale_acc,np.transpose((acc_imu[i,:]-np.transpose(bias_acc)))))
        mag_imu[i,:]=  np.transpose(np.dot(scale_mag,np.transpose((mag_imu[i,:]-np.transpose(bias_mag)))))
        gyr_imu[i,:]=  np.transpose(np.dot(scale_gyr,np.transpose((gyr_imu[i,:]-np.transpose(bias_gyr)))))
        quaternion[i+1]= madgwick.update(quaternion[i],np.hstack([acc_imu[i,:],mag_imu[i,:], gyr_imu[i,:]]))
        euler[i]=quat2euler(quaternion[i+1])
    
    #######################
    # Plots
    ####################### 
    dataVicon = np.loadtxt('data/RIGHT_FOOT.txt',skiprows=4)
    time_vicon = dataVicon[:,0]
    euler_vicon= np.zeros((len(time_vicon),3))
    quat_offset = np.hstack([dataVicon[0,7], dataVicon[0,4], dataVicon[0,5], dataVicon[0,6]])
    for i in range(1, len(time_vicon)-1):
        quat_vicon = np.hstack([dataVicon[i,7], dataVicon[i,4], dataVicon[i,5], dataVicon[i,6]])

        euler_vicon[i] = quat2euler(nq.mult(nq.conjugate(quat_offset),quat_vicon))
