close all
clear all

%addpath('quaternions');

filenm = 'BED_Test_quaternion.txt';
%filenm = 'BED3_20170203quat.txt';



fid = fopen(filenm);

C = textscan(fid,'%s %s %f %f %f %f');

fclose(fid);

q(:,1) = C{3};
q(:,2) = C{4};
q(:,3) = C{5};
q(:,4) = C{6};

idx = find(~isnan(q(:,1)));
q = q(idx,:);
dt = 1/50;


tau = 0.2;

Decimation = 1;
%Apply Decimation
q = q(1:Decimation:end,:);

dt = dt*Decimation;


[InstAxisOfRotation, AngularRate] = ComputeAngularDisplacement(q,dt,tau);

t = dt*(0:length(AngularRate)-1);
TotalRotation = dt*cumtrapz(AngularRate);



figure('Position',[  1058         480        1160         992]);
ah1 = subplot(3,1,1);
plot(t,InstAxisOfRotation);
title(['Quat Rotations Between Timesteps - Low Pass Filter tau = ' num2str(tau) ' s']);
ylabel('Quat Components');
legend('X','Y','Z');
ah2 = subplot(3,1,2);
plot(t,AngularRate);
ylabel('Angular Rotation Rate (degrees/second)');
ah3 = subplot(3,1,3);
TotalRotation = dt*cumtrapz(AngularRate);
plot(t,TotalRotation);
ylabel('Cumulative Rotation (degrees)');
grid on;
linkaxes([ah1 ah2 ah3],'x');

disp(['Total Rotation = ' num2str(TotalRotation(end)) ' deg']);
disp(['Total Rotation = ' num2str(TotalRotation(end)/360) ' rotations']);
disp(['Average Angular Rate = ' num2str(TotalRotation(end)/t(end)) ' deg/sec']);

