function [tn, yn] = rk4(f, t0, tend, y0, h)
%
% Solve the ODE y'=f(t, y) with RK4. 
% Input: f       : Function handle to the rhs. of the ODE. 
%        t0, tend: The integration interval. 
%        y0      : Initial value   
%        h       : Stepsize
% Output: tn     :
%         yn     : The solution at the points tn.

t = t0;    % Current solution
y = y0;

tn = t0;   % Store the solution
yn = y0;

while t <= tend
    if t + h > tend + 1e-6;
        h = tend - t;
    end
    
    % The Improved Euler step
    k1 = feval(f, t, y);
    k2 = feval(f, t + 0.5 * h, y + 0.5 * h * k1);
    k3 = feval(f, t + 0.5 * h, y + 0.5 * h * k2);
    k4 = feval(f, t + h, y + h * k3);
    
    y = y + h * (k1 + 2 * k2 + 2 * k3 + k4) / 6;
    t = t + h;
    
    % Store the solution
    tn = [tn, t];
    yn = [yn, y];
end
