%% Close and read

close all;clear all;clc

test= textread('teste.txt');

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);

phi = (phi - 0)*0.1;
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

%% Plot data
close all

f1=figure('color',[1 1 1],'position',[50 50 600 600]);
f2=figure('color',[1 1 1],'position',[700 50 600 600]);

hold on

for i=2:1:size(time,2)
    figure(f1)
    plot(x(i),y(i),'*r');
    hold on
    
    figure(f2)
    plot(time(i),phi(i),'*b')
    hold on
    
    
    
    pause(0.001)
end

%%
close all
figure('color',[1 1 1],'position',[50 50 600 600]);

plot(x,y,'-*r')
grid on

figure('color',[1 1 1],'position',[700 50 600 600]);

plot(time_real,theta,'-*b')
grid on

figure('color',[1 1 1],'position',[700 50 600 600]);

plot(time_real,vel_real,'-*k')
grid on

figure('color',[1 1 1],'position',[700 50 600 600]);

plot(time_real,phi,'-*g')
grid on
