% [maneuver, time, velocity, displacement, gear];
clear all;
close all
test= textread('teste.txt');
test1= textread('teste1.txt');
test2= textread('teste2.txt');
test3= textread('teste3.txt');
test4= textread('teste4.txt');
test5= textread('teste5.txt');
test6= textread('teste6.txt');
test7= textread('teste7.txt');
test8= textread('teste8.txt');
test9= textread('teste9.txt');

ensaio=0;
for j=1:10
    
    if (ensaio==0)
        test=test;
    end
    if (ensaio==1)
        test=test1;
    end
    if (ensaio==2)
        test=test2;
    end
    if (ensaio==3)
        test=test3;
    end
    if (ensaio==4)
        test=test4;
    end
    if (ensaio==5)
        test=test5;
    end
    if (ensaio==6)
        test=test6;
    end
    if (ensaio==7)
        test=test7;
    end
    if (ensaio==8)
        test=test8;
    end
    if (ensaio==9)
        test=test9;
    end 
    
    increment=1;
    increment1=1;

    for i=1:size(test,1)    
        if test(i,1)==4||test(i,1)==3
            selection(increment,:)=test(i,1:8);
            increment=increment+1;
        end
    end
    for ii=1:size(selection,1)
        time(ii)=selection(ii,2)-selection(1,2);
    end
    for iii=1:size(selection,1)
        displacement(iii)=selection(iii,4)-selection(1,4);
    end
    
    
    
    selection(:,2)=time;
    selection(:,4)=displacement;

    theta(1)=0;
    x(1)=0;
    y(1)=0;
    
    time_mask =logical(diff(time));
    time_real=time(time_mask);
    
    phi = selection(time_mask,8);
    vel = selection(time_mask,3);
    %     timestamp = selection(:,2);

    selection=selection(time_mask,:);
    
    phi = (phi + 0)*0.075;
    vel  = vel *1;
    l=2.56;
    W = 1.5;
    for iii=2:(size(time_real,2))

        R = l/tan(phi(iii));

        vel_real(iii) = vel(iii)*(R/(R-W/2));
        
        dt = time_real(iii)-time_real(iii-1);
        theta(iii)=sin(phi(iii-1))*vel_real(iii)*(dt/l)+theta(iii-1);
        x(iii)=cos(theta(iii-1))*cos(phi(iii-1))*vel_real(iii)*dt+x(iii-1);
        y(iii)=sin(theta(iii-1))*cos(phi(iii-1))*vel_real(iii)*dt+y(iii-1);
    end
  
    selection(:,3)=vel_real;
    
    if (ensaio==0)
        selection0=selection;
        x0=x;
        y0=y;
    end
    if (ensaio==1)
        selection1=selection;
        x1=x;
        y1=y;
    end
    if (ensaio==2)
        selection2=selection;
        x2=x;
        y2=y;
    end
    if (ensaio==3)
        selection3=selection;
        x3=x;
        y3=y;
    end
    if (ensaio==4)
        selection4=selection;
        x4=x;
        y4=y;
    end
    if (ensaio==5)
        selection5=selection;
        x5=x;
        y5=y;
    end
    if (ensaio==6)
        selection6=selection;
        x6=x;
        y6=y;
    end
    if (ensaio==7)
        selection7=selection;
        x7=x;
        y7=y;
    end
    if (ensaio==8)
        selection8=selection;
        x8=x;
        y8=y;
    end
    if (ensaio==9)
        selection9=selection;
        x9=x;
        y9=y;
    end 
    clear selection
    clear time
    clear displacement
    clear test
    clear x
    clear y
    clear time_mask
    clear time_real
    clear phi
    clear vel
    clear R
    clear vel_real
    
    ensaio=ensaio+1;
end


comp=max([selection0(end,2) selection1(end,2) selection2(end,2) selection3(end,2) ...
    selection4(end,2) selection5(end,2) selection6(end,2) selection7(end,2) ...
    selection8(end,2) selection9(end,2)]);
    


figure

plot(selection0(:,2), selection0(:,3),'Color', [0.5 0 0],'linewidth',2)
hold on
plot(selection1(:,2), selection1(:,3),'Color', [0 0.5 0],'linewidth',2)
hold on
plot(selection2(:,2), selection2(:,3),'y','linewidth',2)
hold on
plot(selection3(:,2), selection3(:,3),'m','linewidth',2)
hold on
plot(selection4(:,2), selection4(:,3),'c','linewidth',2)
hold on
plot(selection5(:,2), selection5(:,3),'r','linewidth',2)
hold on
plot(selection6(:,2), selection6(:,3),'g','linewidth',2)
hold on
plot(selection7(:,2), selection7(:,3),'b','linewidth',2)
hold on
plot(selection8(:,2), selection8(:,3),'Color', [0 0 0.5],'linewidth',2)
hold on
plot(selection9(:,2), selection9(:,3),'k','linewidth',2)
hold on
grid on
box off

axis([0 comp -3 3])
xlabel('Tempo [segundos]','fontsize',16);
ylabel('Velocidade [m/s]','fontsize',16);
set(gca,'fontsize',14)

h=legend('Primeiro ensaio','Segundo ensaio', 'Terceiro ensaio', 'Quarto ensaio', 'Quinto ensaio','Sexto ensaio', 'Sétimo ensaio', 'Oitavo ensaio', 'Nono ensaio', 'Décimo ensaio');
set(h,'edgecolor',[1 1 1])

figure

plot(selection0(:,2), selection0(:,4),'Color', [0.5 0 0],'linewidth',2)
hold on
plot(selection1(:,2), selection1(:,4),'Color', [0 0.5 0],'linewidth',2)
hold on
plot(selection2(:,2), selection2(:,4),'y','linewidth',2)
hold on
plot(selection3(:,2), selection3(:,4),'m','linewidth',2)
hold on
plot(selection4(:,2), selection4(:,4),'c','linewidth',2)
hold on
plot(selection5(:,2), selection5(:,4),'r','linewidth',2)
hold on
plot(selection6(:,2), selection6(:,4),'g','linewidth',2)
hold on
plot(selection7(:,2), selection7(:,4),'b','linewidth',2)
hold on
plot(selection8(:,2), selection8(:,4),'Color', [0 0 0.5],'linewidth',2)
hold on
plot(selection9(:,2), selection9(:,4),'k','linewidth',2)
hold on
grid on
box off

axis([0 comp -700 700])
xlabel('Tempo [segundos]','fontsize',16);
ylabel('Deslocamento [incrementos]','fontsize',16);
set(gca,'fontsize',14)

H=legend('Primeiro ensaio','Segundo ensaio', 'Terceiro ensaio', 'Quarto ensaio', 'Quinto ensaio','Sexto ensaio', 'Sétimo ensaio', 'Oitavo ensaio', 'Nono ensaio', 'Décimo ensaio');
set(H,'edgecolor',[1 1 1])

figure


plot(x0, y0,'Color', [0.5 0 0],'linewidth',2)
hold on
plot(x1, y1,'Color', [0 0.5 0],'linewidth',2)
hold on
plot(x2, y2,'y','linewidth',2)
hold on
plot(x3, y3,'m','linewidth',2)
hold on
plot(x4, y4,'c','linewidth',2)
hold on
plot(x5, y5,'r','linewidth',2)
hold on
plot(x6, y6,'g','linewidth',2)
hold on
plot(x7, y7,'b','linewidth',2)
hold on
plot(x8, y8,'Color', [0 0 0.5],'linewidth',2)
hold on
plot(x9, y9,'k','linewidth',2)
hold on
grid on
box off

axis([-3.5 3.5 -3.5 3.5])
axis square
xlabel('Deslocamento em XX','fontsize',16);
ylabel('Deslocamento em YY','fontsize',16);
set(gca,'fontsize',14)

hh=legend('Primeiro ensaio','Segundo ensaio', 'Terceiro ensaio', 'Quarto ensaio', 'Quinto ensaio','Sexto ensaio', 'Sétimo ensaio', 'Oitavo ensaio', 'Nono ensaio', 'Décimo ensaio');
set(hh,'edgecolor',[1 1 1])

plot(0,1,'*r')
plot([-3.5 3.5 3.5 -3.5 -3.5],[2 2 0 0 2],'-r')
