% EXAMPLE - sample usage

load sampledata;

% Perform the decomposition on the velocity (try 1-4 submovements)
[best,bestresult,bestfitresult] = decompose(t,vel);

% Pick the minimum number of submovements that given an error of less
% than 0.02 (in this case, 3)
numsubmovements = find(best<0.02,1);

% Calculate the predicted trajectory for this number of submovements
tangvel = sqrt(vel(:,1).^2 + vel(:,2).^2);
[~,~,~,sumpredicted,predictedx,predictedy] = calculateerrorMJxy(bestresult{numsubmovements},t,vel,tangvel);
t0s = bestresult{numsubmovements}(1:4:end);
[~,inds] = sort(t0s);

% Plot the decomposition
colors = {'g','r','m','c'};
subplot(1,3,1);
for m=1:size(predictedx,1)
    plot(t,predictedx(inds(m),1:length(t)),colors{m},'LineWidth',3);
    titles{m} = sprintf('Submovement %d',m);
    hold on;
end
plot(t,sumpredicted(1:length(t),1),'k');
plot(t,vel(:,1),'b');
titles{m+1} = 'Predicted';
titles{m+2} = 'Data';
xlabel('time (s)');
ylabel('x velocity (m/s)');
legend(titles);
ylim([-1 0.5]);
title('x velocity');

subplot(1,3,2);
for m=1:size(predictedy,1)
    plot(t,predictedy(inds(m),1:length(t)),colors{m},'LineWidth',3);
    hold on;
end
plot(t,sumpredicted(1:length(t),2),'k');
plot(t,vel(:,2),'b');
xlabel('time (s)');
ylabel('y velocity (m/s)');
title('y velocity');

subplot(1,3,3);
plot(pos(:,1),pos(:,2),'b');
hold on;
% Convert the predictions to mm
predx = cumtrapz(t,sumpredicted(1:length(t),1)) + pos(1,1);
predy = cumtrapz(t,sumpredicted(1:length(t),2)) + pos(1,2);
plot(predx,predy,'k');
xlabel('x (m)');
ylabel('y (m)');
title('x/y path');

set(gcf,'PaperUnits','inches');
set(gcf,'PaperPosition',[0 0 12 4]);
print('-depsc2','example');
