TIME_EXTRA=3/0.02;

% Loading dos dados
d1=load('control_leg\bothlegs_noload_nocontrol.mat');
d2=load('control_leg\bothlegs_withload_nocontrol.mat');

% Tamanho mínimo
L=min(length(d1.time),length(d2.time));

% Figura 1: Dados da junta do pé
figure(1);
index=1;
scu=1;
joint=2;


% Obtenção dos dados da posição
pos1=zeros(L,1);
for i=1:L, pos1(i)=d1.position(scu,joint,i); end
% Previsão da trajectória
posT1=predict_trajectory(d1.position(scu,joint,1),d1.position(index,joint,end),L,d1.time_ref/(d1.time_ref+TIME_EXTRA));

pos2=zeros(L,1);
for i=1:L, pos2(i)=d2.position(scu,joint,i); end

plot(d1.time(1:L),pos1,d2.time(1:L),pos2,d1.time(1:L),posT1);


% Figura 1: Dados da junta do pé
figure(2);
index=1;
scu=1;
joint=3;

% Obtenção dos dados da posição
pos1=zeros(L,1);
for i=1:L, pos1(i)=d1.position(scu,joint,i); end
% Previsão da trajectória
posT1=predict_trajectory(d1.position(scu,joint,1),d1.position(index,joint,end),L,d1.time_ref/(d1.time_ref+TIME_EXTRA));

pos2=zeros(L,1);
for i=1:L, pos2(i)=d2.position(scu,joint,i); end

plot(d1.time(1:L),pos1,d2.time(1:L),pos2,d1.time(1:L),posT1);


% Figura 1: Dados da junta do pé
figure(3);
index=2;
scu=3;
joint=1;

% Obtenção dos dados da posição
pos1=zeros(L,1);
for i=1:L, pos1(i)=d1.position(index,joint,i); end
% Previsão da trajectória
posT1=predict_trajectory(d1.position(index,joint,1),d1.position(index,joint,end),L,d1.time_ref/(d1.time_ref+TIME_EXTRA));

pos2=zeros(L,1);
for i=1:L, pos2(i)=d2.position(index,joint,i); end

plot(d1.time(1:L),pos1,d2.time(1:L),pos2,d1.time(1:L),posT1);
