%% Fastest path simulation - comparing different paths

%Start at x=0, end at x=1
np=34; %num points
nt=50; %number of times to display
x=linspace(0,1,np); %x values [m]
y=[yy; 0 -0.2*ones(1,np-2) 0; linspace(0, -0.5, np/2) linspace(-0.5, 0, np/2); 0 -0.35*ones(1, np-2) 0]; %y values for paths to compare [m]

ny=size(y, 1); %number of y-curves to compare
gm=9.8; %gravitational acceleration divided by mass [m/kg.s^2]

v=zeros(ny, np); %velocity of particles at given point
t=zeros(ny, np); %times at which the particle reaches xy coordinate on paths to compare

for(i=2:np) %iterations over x
    
    for(j=1:ny) %iterate over paths to compare    
        dp=sqrt((y(j, i-1)-y(j, i))^2+(x(i-1)-x(i))^2);
        hp=atan2(y(j, i-1)-y(j, i), x(i)-x(i-1));
        ap=sin(hp)*gm;
        if(ap==0) %the quadratic solution does not work when the quadratic term is zero
            dt=dp/v(j, i-1);
        else
            dt=real((sqrt(v(j, i-1)^2+2*ap*dp)-v(j, i-1))/ap);
        end
        v(j, i)=v(j, i-1)+ap*dt;
        t(j, i)=t(j, i-1)+dt;
    end

end

%% Now calculate equal-time coordinates

ts=linspace(0, max(max(t)), nt); %have a unified time coordinate
xs=zeros(ny,nt);
ys=zeros(ny,nt);

for(j=1:ny)
    xs(j,:)=interp1(t(j,:), x, ts);
    ys(j,:)=interp1(t(j,:), y(j,:), ts);
    
    xs(isnan(xs))=x(end); %some particles will arrive at early times giving NaN for interpolation
    ys(isnan(ys))=0;
end

%% Now plot comparison
for(i=1:nt)
    for(j=1:ny)
        plot(x,y(j,:));
        hold on
        plot(xs(j,i), ys(j,i), 'o');
    end
    
    axis([0 1 min(min(y)) 0]);
    title(sprintf('t=%f', ts(i)));
    pause(0.1)
    hold off
    
%     frame=getframe(1);
%     img=frame2im(frame);
%     [A,map]=rgb2ind(img,2);
%     if(i==1)
%         imwrite(A,map,'comp.gif','gif','LoopCount',Inf,'DelayTime',0.5);
%     else
%         imwrite(A,map,'comp.gif','gif','WriteMode','append','DelayTime',0.05);
%     end
end