function paperplane_solution
% solution to exercise sheet 2 OCE 2015, Jonas Koenemann

    clear all; close all;

    m = 2.0;
    AR = 10;
    rho = 1.2;
    g = 9.8;
    Sref = 0.5;

    p = [m,AR,rho,g,Sref];


    % INTEGRATION METHODS

    x0 = [0;0;10;0];
    u = 5 * pi/180;

    % ode 45
    tic
    [t,x_ode45] = ode45(@(t,x) ode(t,x,u,p), [0,10], x0);
    ode45_time = toc;
    fprintf('ode45 time with mean timestep %f: %f\n', mean(diff(t)), ode45_time)
    

    % euler integration
    tic
    h_eul = 0.001;
    T = 10.;
    x_eul = [x0];
    for i=0:T/h_eul-1
        dx = ode(0,x_eul(:,end),u,p);
        x_eul(:,end+1) = x_eul(:,end) + dx * h_eul;
    end
    eul_time = toc;
    fprintf('euler time with timestep %.4f: %f\n', h_eul, eul_time)


    % rk4 integrator
    h_rk4 = 0.1;
    x_rk4 = [x0];
    tic
    for i=0:T/h_rk4-1
        x_cur = x_rk4(:,end);
        k1 = ode(0,x_cur,u,p);
        k2 = ode(0,x_cur+h_rk4/2.*k1,u,p);
        k3 = ode(0,x_cur+h_rk4/2.*k2,u,p);
        k4 = ode(0,x_cur+h_rk4*k3,u,p);
        x_rk4(:,end+1) = x_cur + h_rk4/6. * (k1+2*k2+2*k3+k4);
    end
    
    rk4_time = toc;
    fprintf('RK4 time with timestep %.4f: %f\n', h_rk4, rk4_time)
    
    
    figure(1)
    hold on
    
    subplot(4,1,1)
    plot(t, x_ode45(:,1));
    hold on
    plot(0:h_eul:T, x_eul(1,:));   
    plot(0:h_rk4:T, x_rk4(1,:));
    legend('ode45','euler', 'rk4')   
    
    xlabel('time')
    ylabel('px')
    
    subplot(4,1,2)
    plot(t, x_ode45(:,2));
    hold on 
    plot(0:h_eul:T, x_eul(2,:));
    plot(0:h_rk4:T, x_rk4(2,:));
    legend('ode45','euler', 'rk4')     
    
    xlabel('time')
    ylabel('pz')
    
    subplot(4,1,3)
    plot(t, x_ode45(:,3));
    hold on
    plot(0:h_eul:T, x_eul(3,:));
    plot(0:h_rk4:T, x_rk4(3,:));
    legend('ode45','euler', 'rk4')  
    
    xlabel('time')
    ylabel('vx')
    
    subplot(4,1,4)
    plot(t, x_ode45(:,4));
    hold on
    plot(0:h_eul:T, x_eul(4,:));
    plot(0:h_rk4:T, x_rk4(4,:));
    legend('ode45','euler', 'rk4')   
    
    xlabel('time')
    ylabel('vz')
    
    figure(2)
    plot(x_ode45(:,1), -x_ode45(:,2));
    hold on
    plot(x_eul(1,:), -x_eul(2,:));
    plot(x_rk4(1,:), -x_rk4(2,:));
    
    legend('ode45','euler', 'rk4')   
    xlabel('x position')
    ylabel('altitude')
    
    
    
    % simulate for varouse timesteps
    T =1;
    % ground truth
    options = odeset('RelTol',1.e-12,'AbsTol',1.e-15); 
    [t,x_true] = ode45(@(t,x) ode(t,x,u,p), [0,T], x0, options);
    x_true = x_true(end,:)';
    
    errors_rk4 = [];
    errors_eul = [];
    hes =[];
    
    for i=0:4
        h = 10^-i;
        hes(end+1) = h;
        
        % rk4
        tic
        x_rk4 = x0;
        for j=0:T/h-1
            k1 = ode(0,x_rk4,u,p);
            k2 = ode(0,x_rk4+h/2.*k1,u,p);
            k3 = ode(0,x_rk4+h/2.*k2,u,p);
            k4 = ode(0,x_rk4+h*k3,u,p);
            x_rk4 = x_rk4 + h/6. * (k1+2*k2+2*k3+k4);
        end
        toc
        
        % euler 
        tic
        x_eul = x0;
        for j=0:T/h-1
            dx = ode(0,x_eul,u,p);
            x_eul = x_eul + dx * h;
        end
        toc
        
        % compute errors
        error_rk4 = norm(x_true - x_rk4);
        errors_rk4(end+1) = error_rk4;

        error_eul = norm(x_true - x_eul);
        errors_eul(end+1) = error_eul;


        
    end
    
    figure(3)
    loglog(hes, errors_rk4,'-o')
    hold on 
    loglog(hes, errors_eul,'-o')
    xlabel('timestep h')
    ylabel('Error')
    legend('rk4','euler')
    
    
    
    
    
    % LINEARIZE
    delta = 0.001;
    
    x_lin = [10;3;11;5];
    
    f_lin = ode(0,x_lin,u,p);
    
    A1 = (ode(0,x_lin + [delta;0;0;0],u,p) - f_lin) / delta;
    A2 = (ode(0,x_lin + [0;delta;0;0],u,p) - f_lin) / delta;
    A3 = (ode(0,x_lin + [0;0;delta;0],u,p) - f_lin) / delta;
    A4 = (ode(0,x_lin + [0;0;0;delta],u,p) - f_lin) / delta;
    A = [A1 A2 A3 A4]
    
    B = (ode(0,x_lin,u+delta,p) - f_lin) / delta
    
    eigenvalues = eig(A)
    [V,D] = eig(A);
    
    
    
    % wtih rk4
    
    delta = 0.001;
    h = 0.1;
    
    x_lin = [10;3;11;5];
    
    f_lin = rk4_step(x_lin,u,p,h);
    
    A1 = (rk4_step(x_lin + [delta;0;0;0],u,p, h) - f_lin) / delta;
    A2 = (rk4_step(x_lin + [0;delta;0;0],u,p, h) - f_lin) / delta;
    A3 = (rk4_step(x_lin + [0;0;delta;0],u,p, h) - f_lin) / delta;
    A4 = (rk4_step(x_lin + [0;0;0;delta],u,p, h) - f_lin) / delta;
    Ad = [A1 A2 A3 A4]
    
    Bd = (rk4_step(x_lin,u+delta,p,h) - f_lin) / delta
    
    eigenvalues = eig(Ad)
    [V,D] = eig(Ad);
    
    
end

function x_next = rk4_step(x,u,p, h)
    k1 = ode(0,x,u,p);
    k2 = ode(0,x+h/2.*k1,u,p);
    k3 = ode(0,x+h/2.*k2,u,p);
    k4 = ode(0,x+h*k3,u,p);
    x_next = x + h/6. * (k1+2*k2+2*k3+k4);
end


function dx = ode(t,x,u,p)

    px = x(1);
    pz = x(2);
    vx = x(3);
    vz = x(4);
    
    alpha = u;
    
    m = p(1);
    AR = p(2);
    rho = p(3);
    g = p(4);
    Sref = p(5);
    
    speed = norm([vx;vz]);
    
    CL = 2*pi*alpha*10./12.;
    CD = 0.01 + CL*CL/(AR*pi);
    
%     eL = [vz;-vx] / speed;
%     eD = -[vx;vz] / speed;
%     q = .5*rho*speed^2*Sref;
%     F = q * CL * eL + q * CD * eD + [0;m*g];
    
    Fx = .5*rho*speed^2 * CL * Sref*vz/speed ...
         - .5*rho*speed^2* CD * Sref*vx/speed;
      
    Fz = .5*rho*speed^2 * CL * Sref*(-vx)/speed ...
         - .5*rho*speed^2* CD * Sref*vz/speed ...
         + m*g;
      
    dx = [vx; ...
          vz; ...
          Fx/m; ...
          Fz/m];

end