'''
Utilities for movements recordings with inertial measurement units (IMUs)
Currently data from the following systems are supported

    - XIO
    - XSens
    - YEI
    - Polulu
'''

'''
Author: Thomas Haslwanter
Version: 1.8
Date: Oct-2016
'''

import numpy as np
import scipy as sp
from scipy import constants
from scipy.integrate import cumtrapz
import matplotlib.pyplot as plt
import pandas as pd 
from numpy import r_, sum
import re
import os
#import easygui

# The following construct is required since I want to run the module as a script
# inside the skinematics-directory
import os
import sys
sys.path.append( os.path.join( os.path.dirname(__file__), os.path.pardir ) ) 

from skinematics import quat, vector, misc

class IMU:
    '''
    Class for working with working with inertial measurement units (IMUs)
    
    An IMU object can be initialized
        - by giving a filename
        - by providing measurement data and a samplerate
        - without giving any parameter; in that case the user is prompted
          to select an infile

    Parameters
    ----------
    inFile : string
        path- and file-name of data file
    inType : string
        Description of the type of the in-file. Has to be one of the following:
            - xio
            - XSens
            - yei
            - polulu
    inData : dictionary
        The following fields are required:

        acc : (N x 3) array
             Linear acceleration [m/s^2], in the x-, y-, and z-direction
        omega : (N x 3) array
             Angular velocity [deg/s], about the x-, y-, and z-axis
        [mag] : (N x 3) array (optional)
             Local magnetic field, in the x-, y-, and z-direction
        rate: integer
            sample rate [Hz]

    Notes
    -----
    
    IMU-Properties:
        - source
        - acc
        - omega
        - mag
        - rate
        - totalSamples
        - duration
        - dataType

    Examples
    --------
	>>> myimu = IMU(r'tests/data/data_xsens.txt', inType='XSens')
	>>> 
	>>> initialOrientation = np.array([[1,0,0],
	>>>                                [0,0,-1],
	>>>                                [0,1,0]])
	>>> initialPosition = np.r_[0,0,0]
	>>> 
	>>> myimu.calc_orientation(initialOrientation)
	>>> q_simple = myimu.quat[:,1:]
	>>> 
	>>> calcType = 'Madgwick'
	>>> myimu.calc_orientation(initialOrientation, type=calcType)
	>>> q_Kalman = myimu.quat[:,1:]
 
    '''


    def __init__(self, inFile = None, inType='XSens', inData = None):
        '''Initialize an IMU-object'''

        if inData is not None:
            self.setData(inData)
        else: 
            if inFile is None:
                inFile = self._selectInput()
            if os.path.exists(inFile):    
                self.source = inFile

                try:
                    data = import_data(inFile=self.source, inType=inType, paramList=['rate', 'acc', 'omega', 'mag'])
                    self.rate = data[0]
                    self.acc= data[1]
                    self.omega = data[2]
                    self.mag = data[3]
                    self._setInfo()

                    print('data read in!')
                except IOError:
                    print('Could not read ' + inFile)
            else:
                print(inFile + ' does NOT exist!')

    def setData(self, data):
        ''' Set the properties of an IMU-object. '''

        if 'rate' not in data.keys():
            print('Set the "rate" to the default value (100 Hz).')
            data['rate'] = 100.0

        self.rate = data['rate']
        self.acc= data['acc']
        self.omega = data['omega']
        if 'mag' in data.keys():
            self.mag = data['mag']
        self.source = None
        self._setInfo()

    def calc_orientation(self, R_initialOrientation, method='quatInt'):
        '''
        Calculate the current orientation

        Parameters
        ----------
        R_initialOrientation : 3x3 array
                approximate alignment of sensor-CS with space-fixed CS
        type : string
                - 'quatInt' .... quaternion integration of angular velocity
                - 'Kalman' ..... quaternion Kalman filter
                - 'Madgwick' ... gradient descent method, efficient
                - 'Mahony' ....  formula from Mahony, as implemented by Madgwick

        '''

        initialPosition = np.r_[0,0,0]

        if method == 'quatInt':
            (quaternion, position) = calc_QPos(R_initialOrientation, self.omega, initialPosition, self.acc, self.rate)

        elif method == 'Kalman':
            self._checkRequirements()
            quaternion =  kalman_quat(self.rate, self.acc, self.omega, self.mag)

        elif method == 'Madgwick':
            self._checkRequirements()
                    
            # Initialize object
            AHRS = MadgwickAHRS(SamplePeriod=1./self.rate, Beta=0.1)
            quaternion = np.zeros((self.totalSamples, 4))
            
            # The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
            Gyr = self.omega
            Acc = vector.normalize(self.acc)
            Mag = vector.normalize(self.mag)
            
            #for t in range(len(time)):
            for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
                AHRS.Update(Gyr[t], Acc[t], Mag[t])
                quaternion[t] = AHRS.Quaternion
                
        elif method == 'Mahony':
            self._checkRequirements()
                    
            # Initialize object
            AHRS = MahonyAHRS(SamplePeriod=1./np.float(self.rate), Kp=0.5)
            quaternion = np.zeros((self.totalSamples, 4))
            
            # The "Update"-function uses angular velocity in radian/s, and only the directions of acceleration and magnetic field
            Gyr = self.omega
            Acc = vector.normalize(self.acc)
            Mag = vector.normalize(self.mag)
            
            #for t in range(len(time)):
            for t in misc.progressbar(range(self.totalSamples), 'Calculating the Quaternions ', 25) :
                AHRS.Update(Gyr[t], Acc[t], Mag[t])
                quaternion[t] = AHRS.Quaternion
            
        else:
            print('Unknown orientation type: {0}'.format(method))
            return

        self.quat = quaternion

    def calc_position(self, initialPosition):
        '''Calculate the position, assuming that the orientation is already known.'''

        # Acceleration, velocity, and position ----------------------------
        # From q and the measured acceleration, get the \frac{d^2x}{dt^2}
        g = constants.g
        g_v = np.r_[0, 0, g] 
        accReSensor = self.acc - vector.rotate_vector(g_v, quat.q_inv(self.quat))
        accReSpace = vector.rotate_vector(accReSensor, self.quat)

        # Position and Velocity through integration, assuming 0-velocity at t=0
        vel = np.nan*np.ones_like(accReSpace)
        pos = np.nan*np.ones_like(accReSpace)

        for ii in range(accReSpace.shape[1]):
            vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./np.float(self.rate), initial=0)
            pos[:,ii] = cumtrapz(vel[:,ii],        dx=1./np.float(self.rate), initial=initialPosition[ii])

        self.pos = pos

    def _checkRequirements(self):
        '''Check if all the necessary variables are available.'''
        required = [ 'rate', 'acc', 'omega', 'mag' ]
    
        for field in required:
            if field not in vars(self):
                print('Cannot find {0} in calc_orientation!'.format(field))
            
    def _selectInput(self):
        '''GUI for the selection of an in-file. '''

        #fullInFile = easygui.fileopenbox(msg='Input data: ', title='Selection', default='*.txt')
        (file, path) = misc.get_file('*.txt', 'Input data: ')
        fullInFile = os.path.join(path, file)
        print('Selection: ' + fullInFile)
        return fullInFile

    def _setInfo(self):
        '''Set the information properties of that IMU'''

        self.totalSamples = len(self.omega)
        self.duration = np.float(self.totalSamples)/self.rate # [sec]
        self.dataType = str(self.omega.dtype)

def import_data(inFile=None, inType='XSens', paramList=['rate', 'acc', 'omega', 'mag']):
    '''
    Read in Rate and stored 3D parameters from IMUs

    Parameters
    ----------
    inFile : string
             Path and name of input file
    type : sensor-type. Has to be either ['XSens', 'xio', 'yei', 'polulu']
    paramList: list of strings
               You can select between ['rate', 'acc', 'omega', 'mag', 'others']

    Returns
    -------
    List, containing
    rate : float
        Sampling rate
    [List of x/y/z Parameter Values]

    Examples
    --------
    >>> data = import_data(fullInFile, type='XSens', paramList=['rate', 'acc', 'omega'])
    >>> rate = data[0]
    >>> accValues = data[1]
    >>> Omega = data[2]
    
    '''

    if inFile is None:
        #inFile = easygui.fileopenbox(msg='Please select an in-file containing XSens-IMU data: ', title='Data-selection', default='*.txt')
        (file, path) = misc.get_file('*.txt', 'Please select an in-file containing XSens-IMU data: ')
        inFile = os.path.join(path, file)

    varList = ['acc', 'omega', 'mag', 'rate', 'others']
    
    dataDict = {}
    for var in varList:
        dataDict[var]=None
    
    if inType == 'XSens':
        from sensors import xsens
        data = xsens.get_data(inFile)
    elif inType == 'xio':
        from sensors import xio
        data = xio.get_data(inFile)
    elif inType == 'yei':
        from sensors import yei
        data = yei.get_data(inFile)
    elif inType == 'polulu':
        from sensors import polulu
        data = polulu.get_data(inFile)
    else:
        raise ValueError
        
    dataDict['rate'] = data[0]
    dataDict['acc']  = data[1]
    dataDict['omega']  = data[2]
    dataDict['mag']  = data[3]
    
    returnValues = []
    
    # By default, return all values, in alphabetical order
    if paramList == []:
        paramList = list(dataDict.keys())
        paramList.sort()
        print('Return-values: {0}'.format(paramList))
        
    for param in paramList:
        try:
            returnValues.append(dataDict[param])
        except KeyError as err:
            print('Please check the parameter {0}'.format(param))
            print(err)
            
    return returnValues
        
        
def _read_xsens(inFile):
    '''Read data from an XSens sensor.
    The data returned are (in that order): [rate, acceleration, angular_velocity, mag_field_direction]'''
    
    # Get the sampling rate from the second line in the file
    try:
        fh = open(inFile)
        fh.readline()
        line = fh.readline()
        rate = np.float(line.split(':')[1].split('H')[0])
        fh.close()
        returnValues = [rate]

    except FileNotFoundError:
        print('{0} does not exist!'.format(inFile))
        return -1

    # Read the data
    data = pd.read_csv(inFile,
                       sep='\t',
                       skiprows=4, 
                       index_col=False)

    # Extract the columns that you want, by name
    paramList=['Acc', 'Gyr', 'Mag']
    for param in paramList:
        Expression = param + '*'
        returnValues.append(data.filter(regex=Expression).values)

    return returnValues

def _read_xio(inFile):
    '''Read data from an XIO sensor.
    The data returned are (in that order): [rate, acceleration, angular_velocity, mag_field_direction, packet_nr]'''
    
    # XIO-definition of the sampling rate 
    # Get the "Register"
    inFile = '_'.join(inFile.split('_')[:-1]) + '_Registers.csv'
    
    # Make sure that we read in the correct one
    inFile = '_'.join(inFile.split('_')[:-1]) + '_CalInertialAndMag.csv'
    data = pd.read_csv(inFile)
    
    # Generate a simple list of column names
    rate = 256  # currently hardcoded
    
    returnValues = [rate]
    
    # Extract the columns that you want, by name
    paramList=['Acc', 'Gyr', 'Mag', 'Packet']
    for Expression in paramList:
        returnValues.append(data.filter(regex=Expression).values)
        
    return returnValues    

def _read_yei(inFile):
    '''Read data from an YEI sensor.
    The data returned are (in that order): [rate, acceleration, angular_velocity, mag_field_direction]'''
    
    data = pd.read_csv(inFile)
    
    # Generate a simple list of column names
    newColumns = []
    pattern = re.compile('.*%(\w+)\((\w+)\)')
    for name in data.columns:
        newColumns.append(pattern.match(name).groups()[1])
    data.columns = newColumns
    
    
    # Calculate rate (ChipTime is in microsec)
    start = data.ChipTimeUS[0] * 1e-6    # microseconds to seconds
    stop = data.ChipTimeUS.values[-1] * 1e-6    # pandas can't count backwards
    rate = len(data) / (stop-start)
    
    returnValues = [rate]
    
    # Extract the columns that you want, by name
    paramList=['Accel', 'Gyro', 'Compass']
    for Expression in paramList:
        returnValues.append(data.filter(regex=Expression).values)
        
    return returnValues

    
def calc_QPos(R_initialOrientation, omega, initialPosition, accMeasured, rate):
    ''' Reconstruct position and orientation, from angular velocity and linear acceleration.
    Assumes a start in a stationary position. No compensation for drift.

    Parameters
    ----------
    omega : ndarray(N,3)
        Angular velocity, in [rad/s]
    accMeasured : ndarray(N,3)
        Linear acceleration, in [m/s^2]
    initialPosition : ndarray(3,)
        initial Position, in [m]
    R_initialOrientation: ndarray(3,3)
        Rotation matrix describing the initial orientation of the sensor,
        except a mis-orienation with respect to gravity
    rate : float
        sampling rate, in [Hz]

    Returns
    -------
    q : ndarray(N,3)
        Orientation, expressed as a quaternion vector
    pos : ndarray(N,3)
        Position in space [m]

    Example
    -------
    >>> q1, pos1 = calc_QPos(R_initialOrientation, omega, initialPosition, acc, rate)

    '''

    # Transform recordings to angVel/acceleration in space --------------

    # Orientation of \vec{g} with the sensor in the "R_initialOrientation"
    g = 9.81
    g0 = np.linalg.inv(R_initialOrientation).dot(r_[0,0,g])

    # for the remaining deviation, assume the shortest rotation to there
    q0 = vector.q_shortest_rotation(accMeasured[0], g0)    
    R0 = quat.convert(q0, 'rotmat')

    # combine the two, to form a reference orientation. Note that the sequence
    # is very important!
    R_ref = R_initialOrientation.dot(R0)
    q_ref = quat.rotmat2quat(R_ref)

    # Calculate orientation q by "integrating" omega -----------------
    q = quat.calc_quat(omega, q_ref, rate, 'bf')

    # Acceleration, velocity, and position ----------------------------
    # From q and the measured acceleration, get the \frac{d^2x}{dt^2}
    g_v = r_[0, 0, g] 
    accReSensor = accMeasured - vector.rotate_vector(g_v, quat.q_inv(q))
    accReSpace = vector.rotate_vector(accReSensor, q)

    # Make the first position the reference position
    q = quat.q_mult(q, quat.q_inv(q[0]))

    # compensate for drift
    #drift = np.mean(accReSpace, 0)
    #accReSpace -= drift*0.7

    # Position and Velocity through integration, assuming 0-velocity at t=0
    vel = np.nan*np.ones_like(accReSpace)
    pos = np.nan*np.ones_like(accReSpace)

    for ii in range(accReSpace.shape[1]):
        vel[:,ii] = cumtrapz(accReSpace[:,ii], dx=1./rate, initial=0)
        pos[:,ii] = cumtrapz(vel[:,ii],        dx=1./rate, initial=initialPosition[ii])

    return (q, pos)

def kalman_quat(rate, acc, omega, mag):
    '''
    Calclulate the orientation from IMU magnetometer data.

    Parameters
    ----------
    rate : float
    	   sample rate [Hz]	
    acc : (N,3) ndarray
    	  linear acceleration [m/sec^2]
    omega : (N,3) ndarray
    	  angular velocity [rad/sec]
    mag : (N,3) ndarray
    	  magnetic field orientation

    Returns
    -------
    qOut : (N,4) ndarray
    	   unit quaternion, describing the orientation relativ to the coordinate system spanned by the local magnetic field, and gravity

    Notes
    -----
    Based on "Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking" Yun, X. and Bachman, E.R., IEEE TRANSACTIONS ON ROBOTICS, VOL. 22, 1216-1227 (2006)

    '''

    numData = len(acc)

    # Set parameters for Kalman Filter
    tstep = 1./rate
    tau = [0.5, 0.5, 0.5]	# from Yun, 2006

    # Initializations 
    x_k = np.zeros(7)	# state vector
    z_k = np.zeros(7)   # measurement vector
    z_k_pre = np.zeros(7)
    P_k = np.matrix( np.eye(7) )		 # error covariance matrix P_k

    Phi_k = np.matrix( np.zeros((7,7)) ) # discrete state transition matrix Phi_k
    for ii in range(3):
        Phi_k[ii,ii] = np.exp(-tstep/tau[ii])

    H_k = np.matrix( np.eye(7) )		# Identity matrix

    Q_k = np.matrix( np.zeros((7,7)) )	# process noise matrix Q_k
    D = 0.0001*r_[0.4, 0.4, 0.4]		# [rad^2/sec^2]; from Yun, 2006
                                                                            # check 0.0001 in Yun
    for ii in range(3):
        Q_k[ii,ii] =  D[ii]/(2*tau[ii])  * ( 1-np.exp(-2*tstep/tau[ii]) )

    # Evaluate measurement noise covariance matrix R_k
    R_k = np.matrix( np.zeros((7,7)) )
    r_angvel = 0.01;      # [rad**2/sec**2]; from Yun, 2006
    r_quats = 0.0001;     # from Yun, 2006
    for ii in range(7):
        if ii<3:
            R_k[ii,ii] = r_angvel
        else:
            R_k[ii,ii] = r_quats

    # Calculation of orientation for every time step
    qOut = np.zeros( (numData,4) )

    for ii in range(numData):
        accelVec  = acc[ii,:]
        magVec    = mag[ii,:]
        angvelVec = omega[ii,:]
        z_k_pre = z_k.copy()	# watch out: by default, Python passes the reference!!

        # Evaluate quaternion based on acceleration and magnetic field data
        accelVec_n = vector.normalize(accelVec)
        magVec_hor = magVec - accelVec_n * accelVec_n.dot(magVec)
        magVec_n   = vector.normalize(magVec_hor)
        basisVectors = np.vstack( (magVec_n, np.cross(accelVec_n, magVec_n), accelVec_n) ).T
        quatRef = quat.q_inv(quat.rotmat2quat(basisVectors)).flatten()

        # Update measurement vector z_k
        z_k[:3] = angvelVec
        z_k[3:] = quatRef

        # Calculate Kalman Gain
        # K_k = P_k * H_k.T * inv(H_k*P_k*H_k.T + R_k)
        # Check: why is H_k used in the original formulas?
        K_k = P_k * np.linalg.inv(P_k + R_k)

        # Update state vector x_k
        x_k += np.array( K_k.dot(z_k-z_k_pre) ).ravel()

        # Evaluate discrete state transition matrix Phi_k
        Phi_k[3,:] = r_[-x_k[4]*tstep/2, -x_k[5]*tstep/2, -x_k[6]*tstep/2,              1, -x_k[0]*tstep/2, -x_k[1]*tstep/2, -x_k[2]*tstep/2]
        Phi_k[4,:] = r_[ x_k[3]*tstep/2, -x_k[6]*tstep/2,  x_k[5]*tstep/2, x_k[0]*tstep/2,               1,  x_k[2]*tstep/2, -x_k[1]*tstep/2]
        Phi_k[5,:] = r_[ x_k[6]*tstep/2,  x_k[3]*tstep/2, -x_k[4]*tstep/2, x_k[1]*tstep/2, -x_k[2]*tstep/2,               1,  x_k[0]*tstep/2]
        Phi_k[6,:] = r_[-x_k[5]*tstep/2,  x_k[4]*tstep/2,  x_k[3]*tstep/2, x_k[2]*tstep/2,  x_k[1]*tstep/2, -x_k[0]*tstep/2,               1]

        # Update error covariance matrix
        #P_k = (eye(7)-K_k*H_k)*P_k
        # Check: why is H_k used in the original formulas?
        P_k = (H_k - K_k) * P_k

        # Projection of state quaternions
        x_k[3:] += quat.q_mult(0.5*x_k[3:],r_[0, x_k[:3]]).flatten()
        x_k[3:] = vector.normalize( x_k[3:] )
        x_k[:3] = np.zeros(3)
        x_k[:3] = tstep * (-x_k[:3]+z_k[:3])

        qOut[ii,:] = x_k[3:]

        # Projection of error covariance matrix
        P_k = Phi_k * P_k * Phi_k.T + Q_k

    # Make the first position the reference position
    qOut = quat.q_mult(qOut, quat.q_inv(qOut[0]))
        
    return qOut

class MadgwickAHRS:
    '''Madgwick's gradient descent filter.
    '''
    
    def __init__(self, SamplePeriod=1./256, Beta=1.0, Quaternion=[1,0,0,0]):
        '''Initialization
        
        Parameters
        ----------
        SamplePeriod : sample period
        Beta : algorithm gain
        Quaternion : output quaternion describing the Earth relative to the sensor
        '''
        self.SamplePeriod = SamplePeriod
        self.Beta = Beta
        self.Quaternion = Quaternion
    
    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.'''
        
        q = self.Quaternion; # short name local variable for readability

        # Reference direction of Earth's magnetic field
        h = vector.rotate_vector(Magnetometer, q)
        b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))

        # Gradient decent algorithm corrective step
        F = [2*(q[1]*q[3] - q[0]*q[2])   - Accelerometer[0],
             2*(q[0]*q[1] + q[2]*q[3])   - Accelerometer[1],
             2*(0.5 - q[1]**2 - q[2]**2) - Accelerometer[2],
             2*b[1]*(0.5 - q[2]**2 - q[3]**2) + 2*b[3]*(q[1]*q[3] - q[0]*q[2])   - Magnetometer[0],
             2*b[1]*(q[1]*q[2] - q[0]*q[3])   + 2*b[3]*(q[0]*q[1] + q[2]*q[3])   - Magnetometer[1],
             2*b[1]*(q[0]*q[2] + q[1]*q[3])   + 2*b[3]*(0.5 - q[1]**2 - q[2]**2) - Magnetometer[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 = J.T.dot(F)
        step = vector.normalize(step)	# normalise step magnitude

        # Compute rate of change of quaternion
        qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])) - self.Beta * step

        # Integrate to yield quaternion
        q = q + qDot * self.SamplePeriod
        self.Quaternion = vector.normalize(q).flatten()
        
class MahonyAHRS:
    '''Madgwick's implementation of Mayhony's AHRS algorithm
    '''
    def __init__(self, SamplePeriod=1./256, Kp=1.0, Ki=0, Quaternion=[1,0,0,0]):
        '''Initialization
        
        Parameters
        ----------
        SamplePeriod : sample period
        Kp : algorithm proportional gain
        Ki : algorithm integral gain
        Quaternion : output quaternion describing the Earth relative to the sensor
        '''
        self.SamplePeriod = SamplePeriod
        self.Kp = Kp
        self.Ki = Ki
        self.Quaternion = Quaternion
        self._eInt = [0, 0, 0]  # integral error

    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.'''
        
        q = self.Quaternion; # short name local variable for readability

        # Reference direction of Earth's magnetic field
        h = vector.rotate_vector(Magnetometer, q)
        b = np.hstack((0, np.sqrt(h[0]**2+h[1]**2), 0, h[2]))

        # Estimated direction of gravity and magnetic field
        v = np.array([
             2*(q[1]*q[3] - q[0]*q[2]),
             2*(q[0]*q[1] + q[2]*q[3]),
             q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2])

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

        # Error is sum of cross product between estimated direction and measured direction of fields
        e = np.cross(Accelerometer, v) + np.cross(Magnetometer, w) 
        
        if self.Ki > 0:
            self._eInt += e * self.SamplePeriod  
        else:
            self._eInt = np.array([0, 0, 0], dtype=np.float)
        
        # Apply feedback terms
        Gyroscope += self.Kp * e + self.Ki * self._eInt;            
        
        # Compute rate of change of quaternion
        qDot = 0.5 * quat.q_mult(q, np.hstack([0, Gyroscope])).flatten()

        # Integrate to yield quaternion
        q += qDot * self.SamplePeriod

        self.Quaternion = vector.normalize(q)

if __name__ == '__main__':
    myIMU = IMU(inFile = r'tests/data/data_xsens.txt', inType='XSens')
    myIMU.calc_orientation(np.eye(3), method='Madgwick')
    quat = myIMU.quat[:,1:]
    fig, axs = plt.subplots(3,1)
    axs[0].plot(myIMU.omega)
    axs[0].set_ylabel('Omega')
    axs[1].plot(myIMU.acc)
    axs[1].set_ylabel('Acc')
    axs[2].plot(quat)
    axs[2].set_ylabel('Quat')
    plt.show()
    
    
    '''
    myimu = IMU(r'tests/data/data_xsens.txt')

    initialOrientation = np.array([[1,0,0],
                                   [0,0,-1],
                                   [0,1,0]])

    myimu.calc_orientation(initialOrientation, type='quatInt')
    q_simple = myimu.quat[:,1:]
    
    #calcType = 'Mahony'
    calcType = 'Madgwick'
    calcType = 'Kalman'
    myimu.calc_orientation(initialOrientation, type=calcType)
    q_Kalman = myimu.quat[:,1:]
    
    #myimu.calc_position(initialPosition)

    t = np.arange(myimu.totalSamples)/myimu.rate
    plt.plot(t, q_simple, '-', label='simple')
    plt.hold(True)
    plt.plot(t, q_Kalman, '--', label=calcType)
    plt.legend()
    #plt.plot(t, myimu.pos)
    plt.show()
    '''

    
    print('Done')
