
function vs_show_traj()
close all
clc
clear
%% the parameters of this function

% lag for moving average (0 is no average at all)
winSize = 50 ;

% the allowed step for directions; resolution of slopes (eg. pi/4)
res = pi/4; 

% threshold for data variation dead-zone, that is min joystick displacement accepted. (0 means all variations accepted) 
bThresh=0;

%%  Read and reformat data
F=dlmread('Diagonal0.txt'); F=F';

%% define aux variables
XX=F(1,:);YY=F(2,:);ZZ=F(3,:);
%Zero=zeros(size(ZZ));

%% Calculate moving average of data and their derivatives
newF=moving_average(F,winSize);
dF=diff(newF')';
newdF=moving_average(dF,winSize);

%% adjust dead-zones in derivatives (minimum allowed displacements)
dXX=newdF(1,:); dXX(abs(dXX) < bThresh)=0;
dYY=newdF(2,:); dYY(abs(dYY) < bThresh)=0;
dZZ=newdF(3,:); dZZ(abs(dZZ) < bThresh)=0;

%% Calculate direction of motion ...
angXY=atan2(dYY,dXX);
angZY=atan2(dYY,dZZ);
newangXY=angXY;
newangZY=angZY;

% ... and the relative contributions in fractions of slope resolutions
domXY=round(newangXY/res)
domZY=round(newangZY/res);

%Add the starting point for proper integration
dXY=sqrt(dXX.*dXX+dYY.*dYY);
dZY=sqrt(dZZ.*dZZ+dYY.*dYY);
newdXX=[ XX(1,1) dXY.*cos(res*domXY)];
newdYY=[ YY(1,1) dXY.*sin(res*domXY)];
newdZZ=[ ZZ(1,1) dZY.*cos(res*domZY)];

%% restore X, Y data by proper integration of winning differentials
newXX=cumsum(newdXX);
newYY=cumsum(newdYY);
newZZ=cumsum(newdZZ);

%% overlay the original and reconstructed plots
figure
plot3(XX, YY,ZZ, 'b','LineWidth',2);  hold on; grid on
plot3(newXX, newYY,newZZ, '-r','LineWidth',2);
xlabel('x (mm)','FontSize',14)
ylabel('y (mm)','FontSize',14)
zlabel('z (mm)','FontSize',14)
legend('Position','Filtered Position','FontSize',14)

%% mark every 1000 points to asses differences
%plot3(XX(1:1000:end), YY(1:1000:end),ZZ(1:1000:end), '*b');
%plot3(newXX(1:1000:end), newYY(1:1000:end),newZZ(1:1000:end), 'or');

figure
t=linspace(1,10,7053);
plot(t,XX,'LineWidth',2)
hold on
plot(t,newXX,'--','LineWidth',2)
hold on
plot(t,YY,'r','LineWidth',2)
hold on
plot(t,newYY,'--r','LineWidth',2)
hold on
plot(t,ZZ,'g','LineWidth',2)
hold on
plot(t,newZZ,'--g','LineWidth',2)
legend('Original XX','Filtered XX','Original YY','Filtered YY','Original ZZ','Filtered ZZ','FontSize',14)
xlabel('time(s)','FontSize',14)
ylabel('position (mm)','FontSize',14)
hold off
end

function MA=moving_average(V,winSize)
    %MA=tsmovavg(V, 's', winSize); % creates NaN for positions n < winSize!
    MA=zeros(size(V));
    for n=1:size(V,2)
        MA(:,n)=mean(V(:,max(1,n-winSize):n),2);
    end
end

