%% Fastest path calculator by successive approximation

%Start at x=0, end at x=1
np=34; %num points
x=linspace(0,1,np); %x values [m]
y=linspace[0 -0.2*ones(1,np-2) 0]; %guess initial y values for path [m]
gm=9.8; %gravitational acceleration divided by mass [m/kg.s^2]

dy=0.0001; %test this range about current y value to make an adjustment
ds=0.01; %adjust successive approximation with this scaling

for(n=1:1) %iterations
    v0=0; %initial velocity of 0 at start point
    tt=0; %total time counter
    
    for(i=2:length(x)-1)
        yp=y(i)+dy/2; %first see how long it takes with y+dy/2, both on left side and on right side
        dpl=sqrt((y(i-1)-yp)^2+(x(i-1)-x(i))^2);
        hpl=atan2(y(i-1)-yp, x(i)-x(i-1));
        apl=sin(hpl)*gm;
        tpl=real((sqrt(v0^2+2*apl*dpl)-v0)/apl);
        
        vp=v0+apl*tpl;
        dpr=sqrt((y(i+1)-yp)^2+(x(i+1)-x(i))^2);
        hpr=atan2(yp-y(i+1), x(i+1)-x(i));
        apr=sin(hpr)*gm;
        tpr=real((sqrt(vp^2+2*apr*dpr)-vp)/apr);
        
        tp=tpl+tpr;

        yp=y(i)-dy/2; %then see how long it takes with y-dy/2, both on left side and on right side
        dpl=sqrt((y(i-1)-yp)^2+(x(i-1)-x(i))^2);
        hpl=atan2(y(i-1)-yp, x(i)-x(i-1));
        apl=sin(hpl)*gm;
        tpl=real((sqrt(v0^2+2*apl*dpl)-v0)/apl);
        
        vp=v0+apl*tpl;
        dpr=sqrt((y(i+1)-yp)^2+(x(i+1)-x(i))^2);
        hpr=atan2(yp-y(i+1), x(i+1)-x(i));
        apr=sin(hpr)*gm;
        tpr=real((sqrt(vp^2+2*apr*dpr)-vp)/apr);
        
        tn=tpl+tpr;
        
        dz=(tp-tn)/dy; %then update y in direction of decreasing time
        y(i)=y(i)-dz*ds;

        dp=sqrt((y(i-1)-y(i))^2+(x(i-1)-x(i))^2); %then update v0 for next segment
        hp=atan2(y(i-1)-y(i), x(i)-x(i-1));
        ap=sin(hp)*gm;
        dt=real((sqrt(v0^2+2*ap*dp)-v0)/ap);
        v0=v0+ap*dt;
        tt=tt+dt;
    end
    
    dp=sqrt((y(end-1)-y(end))^2+(x(end-1)-x(end))^2); %calculate end time and velocity
    hp=atan2(y(end-1)-y(end), x(end)-x(end-1));
    ap=sin(hp)*gm;
    dt=real((sqrt(v0^2+2*ap*dp)-v0)/ap);
    v0=v0+ap*dt;
    tt=tt+dt;

%     plot(x,y);
%     title(sprintf('Total t=%f, end v=%f, n=%d', tt, v0, n));
%     pause(0.1)
end
plot(x,y);
    title(sprintf('Total t=%f, end v=%f, n=%d', tt, v0, n));
    pause(0.1)