"""
This file contains the abstract base class "IMU_Base" for analyzing movements
recordings with inertial measurement units (IMUs), as well as functions and
classes for the evaluation of IMU-data..

The advantage of using an "abstract base class" is that it allows to write
code that is independent of the IMU-sensor. All IMUs provide acceleration
and angular velocities, and most of them also the direction of the local
magnetic field. The specifics of each sensor are hidden in the sensor-object
(specifically, in the "get\_data" method which has to be implemented once
for each sensor). Initialization of a sensor object includes a number of
activities:

        - Reading in the data.
        - Making acceleration, angular\_velocity etc. accessible in a sensor-independent way
        - Calculating duration, totalSamples, etc.
        - Calculating orientation (expressed as "quat"), with the method specified in "q\_type"

"""

#Author: Thomas Haslwanter

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd 
import scipy as sp
from scipy import constants     # for "g"
from scipy.integrate import cumtrapz
import re

# 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, rotmat

# For deprecation warnings
# import deprecation
import warnings

# For the definition of the abstract base class IMU_Base
import abc

class IMU_Base(metaclass=abc.ABCMeta):
    '''
    Abstract BaseClass for working with working with inertial measurement units (IMUs)
    A concrete class must be instantiated, which implements "get_data". (See example below.)

    Attributes:
        acc (Nx3 array) : 3D linear acceleration
        dataType (string) : Type of data (commonly float)
        duration (float) : Duration of recording [sec]
        mag (Nx3 array) : 3D orientation of local magnectic field
        omega (Nx3 array) : 3D angular velocity
        pos (Nx3 array) : 3D position
        pos_init (3-vector) : Initial position. default is np.ones(3)
        quat (Nx4 array) : 3D orientation
        q_type (string) : Method of calculation for orientation quaternion
        rate (int) : Sampling rate
        R_init (3x3 array) : Rotation matrix defining the initial orientation. Default is np.eye(3)
        source (str) : Name of data-file
        totalSamples (int) : Number of samples
        vel (Nx3 array) : 3D velocity

    Parameters
    ----------
    inFile : string
        path- and file-name of data file / input source
    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]


    Examples
    --------
    >>> # Set the in-file, initial sensor orientation 
    >>> in_file = r'tests/data/data_xsens.txt'
    >>> initial_orientation = np.array([[1,0,0],
    >>>                                 [0,0,-1],
    >>>                                 [0,1,0]])
    >>>  
    >>> # Choose a sensor 
    >>> from skinematics.sensors.xsens import XSens
    >>> sensor = XSens(in_file=in_file, R_init=initial_orientation)
    >>>  
    >>> # By default, the orientation quaternion gets automatically calculated, using the option "analytical"
    >>> q_analytical = sensor.quat
    >>>  
    >>> # Automatic re-calculation of orientation if "q_type" is changed
    >>> sensor.q_type = 'madgwick'
    >>> q_Madgwick = sensor.quat
    >>>  
    >>> sensor.q_type = 'kalman'
    >>> q_Kalman = sensor.quat
    >>>
    >>> # Demonstrate how to fill up a sensor manually
    >>> in_data = {'rate':sensor.rate,
    >>>         'acc': sensor.acc,
    >>>         'omega':sensor.omega,
    >>>         'mag':sensor.mag}
    >>> my_sensor = MyOwnSensor(in_file='My own 123 sensor.', in_data=in_data)

    '''

    @abc.abstractmethod
    def get_data(self, in_file=None, in_data=None):
        """Retrieve "rate", "acc", "omega", "mag" from the input source
        and set the corresponding values of "self".
        With some sensors, "rate" has to be provided, and is taken from "in_data".
        """

    def __init__(self, in_file = None,
                 q_type='analytical', R_init = np.eye(3),
                 calculate_position=True, pos_init = np.zeros(3),
                 in_data = None ):
        """Initialize an IMU-object.
        Note that this includes a number of activities:
        - Read in the data (which have to be either in "in_file" or in "in_data")
        - Make acceleration, angular_velocity etc. accessible, in a sensor-independent way
        - Calculates duration, totalSamples, etc
        - If q_type==None, data are only read in; otherwise, 3-D orientation is calculated
          with the method specified in "q_type", and stored in the property "quat".
        - If position==True, the method "calc_position" is automatically called, and the
          3D position stored in the propery "pos". (Note that if q_type==None, then
          "position" is set to "False".)

        in_file : string
                Location of infile / input
        q_type : string
                Determines how the orientation gets calculated:
                - 'analytical' .... quaternion integration of angular velocity
                - 'kalman' ..... quaternion Kalman filter
                - 'madgwick' ... gradient descent method, efficient
                - 'mahony' ....  formula from Mahony, as implemented by Madgwick
                - 'None' ... data are only read in, no orientation calculated
        R_init : 3x3 array
                approximate alignment of sensor-CS with space-fixed CS
                currently only used in "analytical"
        calculate_position : Boolean
                If "True", position is calculated, and stored in property "pos".
        pos_init : (,3) vector
                initial position
                currently only used in "analytical"
        in_data : dictionary
                If the data are provided directly, not from a file
                Also used to provide "rate" for "polulu" sensors.
        """

        if in_data is None and in_file is None:
            raise ValueError('Either in_data or in_file must be provided.')

        elif in_file is None:
            # Get the data from "in_data"
            # In that case, "in_data"
            #   - must contain the fields ['acc', 'omega']
            #   - can contain the fields ['rate', 'mag']
            # self.source is set to "None"
            self._set_data(in_data)

        else: 
            # Set rate, acc, omega, mag
            # Note: this is implemented in the concrete class, implenented in 
            # the corresponding module in "sensors"
            self.source = in_file
            self.get_data(in_file, in_data)

        # Set information not determined by the IMU-data
        self.R_init = R_init
        self.pos_init = pos_init

        # Set the analysis method, and consolidate the object (see below)
        # This also means calculating the orientation quaternion!!
        self.q_type = q_type
        
        if q_type != None:
            if calculate_position:
                self.calc_position()

    @property
    def q_type(self):
        """q_type determines how the orientation is calculated.
        If "q_type" is "None", no orientation gets calculated; otherwise,
        the orientation calculation is performed with 
        "_calc_orientation", using the option "q_type".

        It has to be one of the following values:

        * analytical
        * kalman
        * madgwick
        * mahony
        * None

        """
        return self._q_type

    @q_type.setter
    def q_type(self, value):
        allowed_values = ['analytical',
                          'kalman',
                          'madgwick',
                          'mahony',
                          None]
        if value in allowed_values:
            self._q_type = value
            if value == None:
                self.quat = None
            else:
                self._calc_orientation()
        else:
            raise ValueError('q_type must be one of the following: {0}, not {1}'.format(allowed_values, value))


    def _set_data(self, data):
        # Set the properties of an IMU-object directly

        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._set_info()


    def _calc_orientation(self):
        '''
        Calculate the current orientation

        Parameters
        ----------
        type : string
                - 'analytical' .... 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]

        method = self._q_type
        if method == 'analytical':
            (quaternion, position) = analytical(self.R_init, self.omega, initialPosition, self.acc, self.rate) 

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

        elif method == 'madgwick':
            self._checkRequirements()

            # Initialize object
            AHRS = Madgwick(SamplePeriod=1./self.rate, Beta=1.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

        elif method == 'mahony':
            self._checkRequirements()

            # Initialize object
            AHRS = Mahony(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):
        '''Calculate the position, assuming that the orientation is already known.'''

        initialPosition = self.pos_init
        # 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.vel = vel
        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 _set_info(self):
        '''Complete 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 analytical(R_initialOrientation=np.eye(3),
               omega=np.zeros((5,3)),
               initialPosition=np.zeros(3),
               accMeasured=np.column_stack((np.zeros((5,2)), 9.81*np.ones(5))),
               rate=100):
    ''' Reconstruct position and orientation with an analytical solution,
    from angular velocity and linear acceleration.
    Assumes a start in a stationary position. No compensation for drift.

    Parameters
    ----------
    R_initialOrientation: ndarray(3,3)
        Rotation matrix describing the initial orientation of the sensor,
        except a mis-orienation with respect to gravity
    omega : ndarray(N,3)
        Angular velocity, in [rad/s]
    initialPosition : ndarray(3,)
        initial Position, in [m]
    accMeasured : ndarray(N,3)
        Linear acceleration, in [m/s^2]
    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 = analytical(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 = constants.g
    g0 = np.linalg.inv(R_initialOrientation).dot(np.r_[0,0,g])

    # for the remaining deviation, assume the shortest rotation to there
    q0 = vector.q_shortest_rotation(accMeasured[0], g0)    
    
    q_initial = rotmat.convert(R_initialOrientation, to='quat')
    
    # combine the two, to form a reference orientation. Note that the sequence
    # is very important!
    q_ref = quat.q_mult(q_initial, q0)
    
    # 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 = np.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(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*np.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(rotmat.convert(basisVectors, to='quat')).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,:] = np.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,:] = np.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,:] = np.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,:] = np.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:],np.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 Madgwick:
    '''Madgwick's gradient descent filter.

        Parameters
        ----------
        SamplePeriod : double
            sample period [s]
        Beta : double
            algorithm gain
        Quaternion : array, shape (N,4)
            output quaternion describing the Earth relative to the sensor
        '''

    def __init__(self, SamplePeriod=1./256, Beta=1.0, Quaternion=[1,0,0,0]):
        '''Initialization '''
        
        self.SamplePeriod = SamplePeriod
        self.Beta = Beta
        self.Quaternion = Quaternion

    def Update(self, Gyroscope, Accelerometer, Magnetometer):
        '''Calculate the best quaternion to the given measurement values.
        
        Parameters
        ----------
        Gyroscope : array, shape (N,3)
            Angular velocity [rad/s]
        Accelerometer : array, shape (N,3)
            Linear acceleration (Only the direction is used, so units don't matter.)
        Magnetometer : array, shape (N,3)
            Orientation of local magenetic field.
            (Again, only the direction is used, so units don't matter.)
            
        '''

        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 Mahony:
    '''Madgwick's implementation of Mayhony's AHRS algorithm

        Parameters
        ----------
        SamplePeriod : sample period
        Kp : algorithm proportional gain
        Ki : algorithm integral gain
        Quaternion : output quaternion describing the Earth relative to the sensor
    '''
    def __init__(self, SamplePeriod=1./256, Kp=1.0, Ki=0, Quaternion=[1,0,0,0]):
        '''Initialization '''
        
        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.
        
        Parameters
        ----------
        Gyroscope : array, shape (N,3)
            Angular velocity [rad/s]
        Accelerometer : array, shape (N,3)
            Linear acceleration (Only the direction is used, so units don't matter.)
        Magnetometer : array, shape (N,3)
            Orientation of local magenetic field.
            (Again, only the direction is used, so units don't matter.)
            
        '''

        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__':
    '''
    in_file = r'../others/skinematics-invalid-sqrt.txt'
    import pandas as pd
    
    data = pd.read_csv(in_file, delimiter='\t')
    omega = data.filter(regex='Gyr*').values
    acc = data.filter(regex='Acc*').values    
    analytical(omega = omega, accMeasured=acc)
    
    '''
    from sensors.xsens import XSens

    in_file = r'tests/data/data_xsens.txt'
    initial_orientation = np.array([ [1,0,0],
                                     [0,0,-1],
                                     [0,1,0]])

    #in_file = r'tests/data/data_xsens2.txt'
    #initial_orientation = np.array([[0,0,-1],
                                    #[1, 0, 0],
                                    #[0,-1,0]])
    initial_position = np.r_[0,0,0]

    sensor = XSens(in_file=in_file, R_init=initial_orientation, pos_init=initial_position)

        # By default, the orientation quaternion gets automatically calculated, using "analytical"
    q_analytical = sensor.quat

        # Automatic re-calculation of orientation if "q_type" is changed
    sensor.q_type = 'madgwick'
    q_Madgwick = sensor.quat

    sensor.q_type = 'kalman'
    q_Kalman = sensor.quat

    def show_result(imu_data):
        fig, axs = plt.subplots(3,1)
        axs[0].plot(imu_data.omega)
        axs[0].set_ylabel('Omega')
        axs[0].set_title(imu_data.q_type)
        axs[1].plot(imu_data.acc)
        axs[1].set_ylabel('Acc')
        axs[2].plot(imu_data.quat[:,1:])
        axs[2].set_ylabel('Quat')
        plt.show()

    show_result(sensor)

    print('Done')
