% Análise de trajectórias do CoPref em forma de elipse.
% Introduza como argumento um ficheiro de dados .mat.

function analyse_elipse(filename)

close all

scuid=2;
Nrel=2.5;
L=[9.5 9.5];

load(filename);

fprintf('\nCondições da Experiencia:\n');

fprintf('(X Y)=(%g, %g)\nNsamples=%d\t\tNrounds=%d\t\tdelay=%g\nK=[%d %d %d]\n',X,Y,Nsamples,Nrounds,delay,K);

who

% Cálculo do Centro de Pressão com base no valor dos sensores de pressao
% medidos
CoPmeasured=calccop(reaction_store);

% Cálculo do sinal de erro
error=trajxy_store-CoPmeasured;

figure(1)
subplot(2,1,1);
plot(t_store,trajxy_store,'.',t_store,CoPmeasured,'linewidth',2);
grid on
xlabel('t')
ylabel('Force')
subplot(2,1,2);
plot(t_store,error,'linewidth',2);
grid on
xlabel('t')
ylabel('Error')

% Ângulos dos elos com a perna na vertical
% theta_initial=[0 0 0];
% Ângulos dos servos na posição inicial
% servo_initial=convert2servoangles(scuid,theta_initial,Nrel);
% Desvio dos ângulos previstos com o medido inicialmente
% deviation=servo_initial-servo0

theta_array=zeros(N,3);
xy_array=zeros(N,3,2);
for i=1:N
    theta_array(i,:)=convert2jointangles(scuid,servo_store(i,:),Nrel);
    xy=cinematdir(theta_array(i,:),L);
    xy_array(i,:,1)=xy(1,:);
    xy_array(i,:,2)=xy(2,:);
end

figure(2)
subplot(1,2,1);
plot3(xy_array(1:Nsamples,1,1),xy_array(1:Nsamples,2,1),xy_array(1:Nsamples,3,1),'.','linewidth',2);
xlabel('X');
ylabel('Y');
zlabel('Z');
axis equal
grid on
title('Knee')
subplot(1,2,2);
plot3(xy_array(:,1,2),xy_array(:,2,2),xy_array(:,3,2),'.','linewidth',2);
xlabel('X');
ylabel('Y');
zlabel('Z');
axis equal
grid on
title('Hip')
