close all
clear all


addpath('SpinCalc');
addpath('quaternions');

filenm = 'BED_Test_quaternion.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};

q = q(1:1:end,:);

dt = 1/50;
t = dt*(0:length(q(:,1))-1);

EA = SpinCalc('QtoEA321',q,.001,1);
EV = SpinCalc('QtoEV',q,.001,1);


[V,PHI] = qdecomp(q);


figure
ah1 = subplot(3,1,1);
plot(t,EA);
legend('Roll','Pitch','Yaw');
xlabel('time (s)');
ylabel('degrees');
title('Euler Angles');

ah2 = subplot(3,1,2)
plot(t,EV(:,1:3));
%hold all
%plot(t,V);
legend('X','Y','Z');
xlabel('time (s)');
title('Components of Rotation Vector');

ah3 = subplot(3,1,3)
plot(t,EV(:,4));
%hold all
%plot(t,PHI*180/pi);
linkaxes([ah1 ah2 ah2],'x');



QQ = qmult(q,qconj(q(1,:)));

EA = SpinCalc('QtoEA321',QQ,.001,1);
EV = SpinCalc('QtoEV',QQ,.001,1);


[V,PHI] = qdecomp(QQ);


figure
ah1 = subplot(3,1,1);
plot(t,EA);
legend('Roll','Pitch','Yaw');
xlabel('time (s)');
ylabel('degrees');
title('Euler Angles');

ah2 = subplot(3,1,2)
plot(t,EV(:,1:3));
%hold all
%plot(t,V);
legend('X','Y','Z');
xlabel('time (s)');
title('Components of Rotation Vector');

ah3 = subplot(3,1,3)
plot(t,EV(:,4));
hold all
plot(t,PHI*180/pi);
linkaxes([ah1 ah2 ah2],'x');


for i = 2:length(q)
   
    Qdiff = qmult(q(i,:),qconj(q(i-1,:)));
    
    [Vdiff(i,:),PHIdiff(i)] = qdecomp(Qdiff);
    
end

figure('Position',[  1000         364         884        1134]);
ah1 = subplot(3,1,1)
plot(t(2:end),Vdiff(2:end,:));
title('Quat Rotations Between Timesteps - No Smoothing');
ylabel('Quat Components');
legend('X','Y','Z');
ah2 = subplot(3,1,2)
plot(t(2:end),180*PHIdiff(2:end)/pi);
ylabel('Angular Rotaion (degrees)');
ah3 = subplot(3,1,3)
plot(t(2:end),cumsum(180*PHIdiff(2:end))/pi);
ylabel('Cumulative Rotation (degrees)');
grid on;
linkaxes([ah1 ah2 ah3],'x');


%Filter q
hrange = 0.4;
hbias = 0.4;
low  = max(min(hbias - (hrange./2), 1), 0);
high = max(min(hbias + (hrange./2), 1), 0);
%low = 0;
%high = 1;
hrangeLimited = high - low;

y = q(1,:);  %Initial Filter State
qout = zeros(size(q));  %Allocate Filter Output
qout(1,:) = y;

for ii = 2:length(q)
   x = q(ii,:);
   qdiff = qmult(y,qconj(x));  
   [Vdiff,dist] = qdecomp(qdiff);  %Angular distance (radians) between x and y returned in dist
   
   % Normalize dist output to the range [low, high];
   hlpf = (dist./pi).*hrangeLimited + low;
   y = slerp(y',x',hlpf)';
   qout(ii,:) = y;
end

%Plot Smoothed quaternion results

for i = 2:length(q)
   
    Qdiff = qmult(qout(i,:),qconj(qout(i-1,:)));
    
    [Vdiff(i,:),PHIdiff(i)] = qdecomp(Qdiff);
    
end

figure('Position',[  1058         480        1160         992]);
ah1 = subplot(3,1,1)
plot(t(2:end),Vdiff(2:end,:));
title('Quat Rotations Between Timesteps - Smoothed');
ylabel('Quat Components');
legend('X','Y','Z');
ah2 = subplot(3,1,2)
plot(t(2:end),180*PHIdiff(2:end)/pi/.02);
ylabel('Angular Rotation Rate (degrees/second)');
ah3 = subplot(3,1,3)
plot(t(2:end),cumsum(180*PHIdiff(2:end))/pi);
ylabel('Cumulative Rotation (degrees)');
grid on;
linkaxes([ah1 ah2 ah3],'x');
