function X=twobody(X0,t,mu)
%[R;V]=twobody([R0;V0],t-t0,mu)

%Prussing & Conway "Orbital Mechanics"

%check if input is rows or columns, computations are in rows
rowcol=diff(size(t));if rowcol==0;rowcol=-diff(size(X0));end
%switch colums to rows
if rowcol<0;X0=X0';t=t';mu=mu';end

X02=size(X0,2);
%avoid zero errors
t(find(t==0))=realmin;

%Parameters
R0=X0(1:3,:);
V0=X0(4:6,:);
r0=mag(R0);v0=mag(V0);
a=2./r0-v0.^2./mu;
s=dot(R0,V0)./sqrt(mu);
h=mag(cross(R0,V0));e=sqrt(1-h.^2.*a./mu);
rp=(1-e)./a;

%start algorithm
x=mu.*t.^2./rp./(F(sqrt(mu).*t./rp,a,s,r0,sqrt(mu).*t)+sqrt(mu).*t);%x0
n=5;%Laguerre parameter
err=1;iter=0;
while err>1e-8
    iter=iter+1;
    if iter==50;disp('50 iterations');end
    %update x
    dx=n.*F(x,a,s,r0,sqrt(mu).*t)./...
       (dF(x,a,s,r0)+sign(dF(x,a,s,r0)).*sqrt(abs((n-1).^2.*dF(x,a,s,r0).^2-n.*(n-1).*F(x,a,s,r0,sqrt(mu).*t).*ddF(x,a,s,r0))));
    x=x-dx;
    err=max(abs(dx)./x);
end

%convert to R and V
if X02==1
	R=R0*(1-x.^2./r0.*C(a.*x.^2))+V0*(t-x.^3./sqrt(mu).*S(a.*x.^2));
	V=R0*(x.*sqrt(mu)./mag(R)./r0.*(a.*x.^2.*S(a.*x.^2)-1))+V0*(1-x.^2./mag(R).*C(a.*x.^2));
else
    R=(ones(3,1)*(1-x.^2./r0.*C(a.*x.^2))).*R0+(ones(3,1)*(t-x.^3./sqrt(mu).*S(a.*x.^2))).*V0;
    V=(ones(3,1)*(x.*sqrt(mu)./mag(R)./r0.*(a.*x.^2.*S(a.*x.^2)-1))).*R0+(ones(3,1)*(1-x.^2./mag(R).*C(a.*x.^2))).*V0;
end

X=[R;V];

if rowcol<0;X=X';end
return

%Universal Kepler's Eqn. & Derivatives
function Fx=F(x,a,s,r,mt)
Fx=s.*x.^2.*C(a.*x.^2)+(1-r.*a).*x.^3.*S(a.*x.^2)+r.*x-mt;
return

function dFx=dF(x,a,s,r)
dFx=s.*x.*(1-a.*x.^2.*S(a.*x.^2))+(1-r.*a).*x.^2.*C(a.*x.^2)+r;
return

function ddFx=ddF(x,a,s,r)
ddFx=s.*(1-a.*x.^2.*C(a.*x.^2))+(1-r.*a).*x.*(1-a.*x.^2.*S(a.*x.^2));
return

%auxillary functions
function Cy=C(y)
%Cy=0;for n=0:8;Cy=Cy+(-1)^n*y.^n/gamma(2*n+3);end
aa=find(y>0);Cy(aa)=(1-cos(sqrt(y(aa))))./y(aa);
bb=find(y<0);Cy(bb)=(cosh(sqrt(-y(bb)))-1)./-y(bb);
cc=find(y==0);Cy(cc)=1./2;
return

function Sy=S(y)
%Sy=0;for n=0:8;Sy=Sy+(-1)^n*y.^n/gamma(2*n+4);end
aa=find(y>0);Sy(aa)=(sqrt(y(aa))-sin(sqrt(y(aa))))./sqrt(y(aa)).^3;
bb=find(y<0);Sy(bb)=(sinh(sqrt(-y(bb)))-sqrt(-y(bb)))./sqrt(-y(bb)).^3;
cc=find(y==0);Sy(cc)=1./6;
return

function R=mag(r);
R=sqrt(dot(r,r));
return