turtle='5';     % turtle number
files={'pass'}; % names logfiles
copydata=0; %copy new data?
begin=0; %secs?
duration=3;
speed=3;
trace=1;
%% get .txt files from turtle
for i=1:length(files)
    if(copydata)
        if exist(['./' files{i} '.txt'],'file')
            delete(['./' files{i} '.txt']);
        end
        try
            eval(['!scp robocup@turtle' turtle ':/home/robocup/' files{i} '.txt /home/robocup'])
        catch
        end
    end
    eval([files{i} '=dlmread(''' files{i} '.txt'',' ',[0 10000 [] []]);']);
end

%%

%load passtoday.txt
%from c file attacker assist
%/*currentpos , ballpos   , balposest, balvelest , targetxy   , conf,T_age, xb */
%fprintf(pFile,"%f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %d\t %d\t %f\n",currentpos[0],currentpos[1],ballpos[0],ballpos[1],ballpos_est[0],ballpos_est[1],ballvel_est[0],ballvel_est[1],Turtle_Target[0],Turtle_Target[1],conf,TargetAge,xb);

x=pass(:,1);
y=pass(:,2);
xb=pass(:,3);
yb=pass(:,4);
xb_est=pass(:,5);
yb_est=pass(:,6);
vxb_est=pass(:,7);
vyb_est=pass(:,8);
tx=pass(:,9);
ty=pass(:,10);
conf=pass(:,11);
phi=pass(:,12);
stage=pass(:,13);

n=1:length(x);
t=n./1000;

N_CORR_EST  =      200;                                    % 300 number of motion samples correct bal pos estimate */
TOWARDS_BALLVEL =  -0.5;                                  % -0.5 only react if vel ball towards turtle*/
MAX_EST_ERROR  =   0.08;                                 % 0.15 good estimation kalman if posest - pos < MAX_EST_ERROR */

%% observer params tuning/simulation (simulink sim)
%compute states with kalman filter
% if(0) %true:compute new estimation
%     xy_in.time=t;
%     xy_in.signals.values=[xbv ybv];
%     xy_in.signals.dimensions=2;
%     found_in.time=t;
%     found_in.signals.values=found;
%     found_in.signals.dimensions=1;
% 
%     %simulate observer.mdl
% 
%     pnc=diag([2 2 10 10]);
%     ic_eec=eye(4); %process noise covariance
%     mnc=20*eye(2); %measurement noise covariance
%     sim('test_vball_rob_mdl');
% 
%     xbv=squeeze(xy_ov.signals.values(:,1)); %const vel est blokje
%     ybv=squeeze(xy_ov.signals.values(:,2));
% 
%     xb_est=squeeze(xyvxvy_out.signals.values(1,:,:)); %retreive simulated signals
%     yb_est=squeeze(xyvxvy_out.signals.values(2,:,:));
%     vxb_est=squeeze(xyvxvy_out.signals.values(3,:,:));
%     vyb_est=squeeze(xyvxvy_out.signals.values(4,:,:));
%     xbk=xb_est; %compute new estitmation
%     ybk=yb_est;
%     vxbk=vxb_est;
%     vybk=vyb_est;
% end
%est_error=sqrt((xb-xb_est).^2+(yb-yb_est).^2);

%% plot traces
dt=1/1000;
ms=5; %markersize
dn=speed;%set speed of simulation see top 
if(begin==0)
    tracelength=5*dn;
    n=tracelength:dn:length(t);
else
    tracelength=20*dn;
    n=begin*1000:dn:(begin+duration)*1000;
end
if(1)%show movie or not
    figure(1); set(1,'position',[ 560          62        1006         888]);shg;
    for(i=n) %(i=1000:length(t)-100)
        cla;
        hold on;
        for j=0:dn:(tracelength-dn);
            plot(xb(i-j),yb(i-j),'bo','markersize',ms);
            if (conf(i-j)>=N_CORR_EST) %valid estimation of the intercept point
                plot(xb_est(i-j),yb_est(i-j),'ro','markersize',ms,'markerfacecolor','r');
                if(j==0) %plot velocity             
                    plot([xb_est(i-j) xb_est(i-j)+vxb_est(i-j)],[yb_est(i-j) yb_est(i-j)+vyb_est(i-j)],'r-','linewidth',2);
                    %plot([xb(i-j) xb(i-j)+vxb_est(i-j)],[yb(i-j) yb(i-j)+vyb_est(i-j)],'r-','linewidth',2);
                end      
            else
                plot(xb_est(i-j),yb_est(i-j),'mo','markersize',ms);
                if(j==0)%plot velocity   
                    plot([xb_est(i-j) xb_est(i-j)+vxb_est(i-j)],[yb_est(i-j) yb_est(i-j)+vyb_est(i-j)],'r-','linewidth',2);
                    %plot([xb(i-j) xb(i-j)+vxb_est(i-j)],[yb(i-j) yb(i-j)+vyb_est(i-j)],'m-','linewidth',2);
                end
            end
        end
        plot(x(i),y(i),'ko','markersize',20,'markerfacecolor','k');
        plot([x(i) x(i)+sin(phi(i))],[y(i) y(i)+cos(phi(i))],'k-','linewidth',2);

        plot(tx(i),ty(i),'ro','markersize',15);
        plot(tx(i),ty(i),'ro','markersize',12);

        text(-4,5.5,['sample ' num2str(i)]);
        text(-4,5.0,['phi = ' num2str(phi(i))]);
        text(-4,4.5,['vy = ' num2str(vyb_est(i))]);
        text(-4,4.0,['time ' num2str(conf(i))]);
        text(-4,3.0,['stage ' num2str(stage(i))]);
       
        drawnow;%pause(0);
        grid on;axis equal;axis([-5 5 -6 6]);
    end
end
if(begin~=0)
    plot(xb(1000*begin:1000*(begin+duration)),yb(1000*begin:1000*(begin+duration)),'ro-');
end

%%
if(1)
    figure(10);
    plot(1:length(t),xb_est,'m',1:length(t),yb_est,'c',1:length(t),xb,'r',1:length(t),yb,'b',1:length(t),vxb_est,'g',1:length(t),vyb_est,'k');
    legend('xb_est','yb_est','xb','yb','vxb_est','vyb_est');
    %plot(xmv);hold on;plot(x);
end
%%


if(1)
   range=[53 60];%in seconds
   figure(11);
   dist=sqrt((xb-x).^2+(yb-y).^2);
   plot(dist);
   
end



% figure(100);
% plot(t,conf);grid on;

%
%
%
%
%
%
%