function [xout,yout,err]=rkn4k(ypfun,x0,xfinal,y0,y0p,n)

% The Runge Kutta Nystrom RKN4 method of fifth order with four stages 
% 
% The Runge-Kutta-Nystrom method solves the initial value problem 
%
%       y'' = f(x,y),    y(x0) = y0, y'(x0) = y0p
%
% where y and f are a vectors of dimension m (the number of equations).
%
% Input Arguments
%
% ypfun: the right hand side function f(x,y), written in a matlab file 
%        e.g. funtb.m for the two body problem,
% x0, xfinal: the end points of the integration interval,
% y0, y0p: vectors (dimension m) containing the initial conditions,
% h: the step size,
%
% Return Arguments
% xout: a vector (size(n,1)) which contains the evaluation points (n=(xfinal-x0)/h)
% yout: a matrix (size(n,m)) which contains the numerical solution 
%
% Example: The two body problem (Kepler)
%
%  y1''= -y1/(y1^2+y2^2)^(3/2),  y1(0)=1, y1'(0)=0,
%  y2''= -y2/(y1^2+y2^2)^(3/2),  y2(0)=0, y2'(0)=1
%
% The following function is written in the file "funtb.m"
% function f = funtb(x,y)
% par = (y(1)^2+y(2)^2)^(3/2);
% f1 = -y(1)/par;
% f2 = -y(2)/par;
% f = [f1 f2]';
% end
%
% the integration interval is [0,100] so x0=0, xfinal=100
% the initial condition is y0=[1, 0]'; y0p=[0, 1]';
% for step h=0.5 the solver is called as 
%
% [xapp,yapp]=rkn6('funtb',x0,xfinal,y0,y0p,h);
%
% the points of integration are in the vector xapp (0, 0.5, 1.0, 1.5, ...)
% the approximation of y1 is yapp(:,1)
% the approximation of y2 is yapp(:,2)

% The parameters of the method
c1 = 0 ;
c2 = 1/5 ;
c3 = 2/3 ;
c4 = 1 ;
c = [c1  c2  c3  c4]';
a21 = 1/50 ;
a31 = -1/27 ;
a32 = 7/27 ;
a41 = 3/10 ;
a42 = -2/35 ;
a43 = 9/35 ;

b1 = 14/336 ;
b2 = 100/336 ;
b3 = 54 /336 ;
b4 = 0 ;

bh1 = 14/336 ;
bh2 = 125/336 ;
bh3 = 162/336 ;
bh4 = 35/336 ;

a = [[0      0    0    0  ]
     [a21    0    0    0  ]
     [a31   a32   0    0  ]
     [a41   a42  a43   0  ] ]';
 
b = [b1 b2 b3 b4]';
bh = [bh1  bh2  bh3  bh4 ]';

x = x0;

h = (xfinal-x0)/n;
y = y0(:);
yp = y0p(:);
f=zeros(length(y),4);
xout = zeros(n+1,1);
yout = zeros(n+1,length(y));
k = 1;
xout(k) = x;
yout(k,:) = y.';
err = zeros(n+1,1);
err(1) = 0 ;

for i=1:n

for j = 1:4
temp = feval(ypfun, x + c(j)*h, y + c(j)*h*yp +h^2*f*a(:,j));
f(:,j) = temp(:);
end

y1 = y+h*yp+h^2*f*b;
y1p = yp + h*f*bh;

k = i+1;
x = x+h;
y = y1;
yp = y1p;

xout(k) = x;
yout(k,:) = y.';

% kepler
 err(k) = sqrt((y(1)-cos(x))^2+(y(2)-sin(x))^2);

end
