function str=demo_strelba()
% Demo: Direct multiple shooting method
    
    h   = 0.01;
    crs = 1.0; % crs = c * rho * s
    m   = 1.0;
    g   = 1.0;

    % Target position is (target, 0)
    target  = 2;

    % Min and max elevation angles
    angmin = 5;
    angmax = 45;
    
    imax = 14;
    xf   = zeros(imax,1);
    
    figure
    hold on

    for i=1:imax

        if (i==1) 
            % Try angle angmax
            ang0 = angmax;
        elseif (i==2) 
            % Try angle angmin
            ang0 = angmin;
        elseif (i==3) 
            % Try their average
            ang0 = (angmin+angmax) / 2.0;
        else
            if (xf(i-1)<0)
                % Too close. Increase angle (take [ang0,angmax])
                angmin = ang0;
            else
                % Too far. Decrease angle (take [angmin,ang0])
                angmax = ang0;
            end;
            % Try average of angmin and angmax
            ang0 = (angmin+angmax) / 2.0;
        end
            
        % Initialize the shot
        tn    = 0.0;
        wr(1) = 0.0;
        wr(2) = 0.0;
        wr(3) = 4.0;
        wr(4) = ang0/180.0*pi;

        % Solve the initial problem
        pl=[wr(1) wr(2)];
        while (wr(2)>=0)
            tn = tn+h;
            wr = rk4step(wr,h,crs,m,g);
            pl = [pl;[wr(1) wr(2)]];
        end;
        
        % Print info
        disp(['i= ' num2str(i), ', ang0= ' num2str(ang0), ' deg' ...
              ', tn= ', num2str(tn) ...
              ', (x,y)= (', num2str(wr(1)) ',' num2str(wr(2)) ')' ...
              ', v=' num2str(wr(3)) ', ang=' num2str(wr(4))]);

        xf(i) = wr(1)-target;

        % Plot the trajectory
        plot(pl(:,1),pl(:,2));
        if(i==1)
            xl=xlim(gca);
            plot(xl,[0.0 0.0],'k--');
            plot([target target],[0 0],'rx','Markersize',5,'Linewidth',3)
        end;
        pause
    
    end;
    hold off;
    
    
    
    return;
    
    
%%%%%%%%%%%%
function dwdt = f(w,crs,m,g)
% Right-hand side f(w)
% (Vector of derivatives w.r.t. time)
    
    dwdt = zeros(4,1);
    dwdt(1) = w(3)*cos(w(4));
    dwdt(2) = w(3)*sin(w(4));
    dwdt(3) = -0.5*crs*w(3)^2/m - g*sin(w(4));
    dwdt(4) = -g/w(3)*cos(w(4));
    
    return;
    
%%%%%%%%%%%%

function w = rk4step(w,h,crs,m,g)
% One step by RK4 method
    
    fi  = f(w,crs,m,g);
    k1  = h*fi(:);

    buf = w(:)+k1(:)/2;
    fi  = f(buf,crs,m,g);
    k2  = h*fi(:);

    buf = w(:)+k2(:)/2;
    fi  = f(buf,crs,m,g);
    k3  = h*fi(:);

    buf = w(:)+k3(:)/2;
    fi  = f(buf,crs,m,g);
    k4  = h*fi(:);

    w = w(:) + ( k1(:) + 2.0*k2(:) + 2.0*k3(:) + k4(:) ) / 6.0;
    return;

%%%%%%%%%%%%
   
   
