function [V1,V2,a,N,iteration,index]=lambert(r1,r2,tof,mu,Nrev);
%[V1,V2,a,N,iteration,index]=lambert(r1,r2,tof,mu,Nrev);
%
%Multiple case version
%units and coordinate frame out = units and coordinate frame in
%OUTPUTS
%V1 = velocity vector at t1
%V2 = velocity vectot at t2
%a = semi-major axis of orbit
%N = number of revolutions around central body
%interation = number of iteratoins to converge
%index = list of input indeces corresponding to output column
%INPUTS
%r1 = 3d position at t1
%r2 = 3d position at t2
%tof = time of flight from r1 to r2 (tof > 0 for prograde, tof < 0 for retrograde)
%mu = GM of central body
%Nrev = desired number of revolutions about central body (Nrev > 0 for long period solution, Nrev < 0 for short)
%if Nrev = infinity, then all possible multiple revolution arcs will be found, or
%if specified Nrev value is impossible for corresponding r1,r2, and tof, then that case will be deleted

if nargin<5;Nrev=inf*ones(size(tof));end
if length(Nrev)==1;Nrev=Nrev*ones(size(tof));end

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

nvec=length(tof);
index=[1:nvec]';%set of indeces
N=abs(Nrev);
rhat1=r1./(mag(r1)*ones(1,3));
rhat2=r2./(mag(r2)*ones(1,3));
ta=acos(dot(rhat1',rhat2')');%transfer angle
hhat=cross(rhat1',rhat2')';
%if orbit plane indeterminable, assume hhat=zhat
ii=find(mag(hhat)<1e-12);if ~isempty(ii);hhat(ii,:)=[0 0 1];end
hhat=hhat./(mag(hhat)*ones(1,3));%angular momentum of transfer
%want h to be similar to h of celestial body if prograde , usually in zhat direction
ii=find(dot(hhat',[ones(nvec,1)*[0 0 1]]')'<0);ta(ii)=mod(-ta(ii),2*pi);hhat(ii,:)=-hhat(ii,:);
%want h to be opposite if retrograde
iret=find(tof<0);ta(iret)=mod(-ta(iret),2*pi);hhat(iret,:)=-hhat(iret,:);
tof=abs(tof);
qhat1=cross(hhat',rhat1')';
qhat2=cross(hhat',rhat2')';

c=mag(r2-r1);
s=.5*(c+mag(r1)+mag(r2));
%start Lancaster's Method
T=sqrt(8*mu./s.^3).*tof;%non-dim time
q=sqrt(mag(r1).*mag(r2)).*cos(ta/2)./s;

%find if specified Nrev is possible
aa=find(N>0&N<inf);
if ~isempty(aa)
bb=NorbT(q(aa),T(aa),N(aa));cc=setdiff(aa,aa(bb));%bb works, cc will not work
if length(cc)~=0;
	disp('Warning: Specified number of revs in trajectories:');disp(cc);disp('are not possible and will be deleted');
    %delete trajectories that won't work
    index=setdiff(index,cc);
    Nrev(cc)=[ ];
    N=abs(Nrev);
    q(cc)=[ ];
    T(cc)=[ ];
    s(cc)=[ ];
    c(cc)=[ ];
    r1(cc,:)=[ ];
    r2(cc,:)=[ ];
    rhat1(cc,:)=[ ];
    rhat2(cc,:)=[ ];
    qhat1(cc,:)=[ ];
    qhat2(cc,:)=[ ];
    hhat(cc,:)=[ ];
    if length(bb)==0;disp('Aborting');return;end
end
end

%Find maximum number of revs for given time
zz=find(N==inf);
N(zz)=0;
Nrev(zz)=0;
Ncount=0;
addi=[];
while ~isempty(zz)
zz=zz( NorbT(q(zz),T(zz),(Ncount+1)*ones(length(zz),1)) );
Ncount=Ncount+1;
nzz=length(zz);
%add new multirev cases to end of arrays
addi(end+1:end+2*nzz,1)=[index(zz);index(zz)];
Nrev=[Nrev;(Ncount)*ones(nzz,1); (-Ncount)*ones(nzz,1)];
N=abs(Nrev);
q=[q;q(zz);q(zz)];
T=[T;T(zz);T(zz)];
s=[s;s(zz);s(zz)];
c=[c;c(zz);c(zz)];
r1=[r1;r1(zz,:);r1(zz,:)];
r2=[r2;r2(zz,:);r2(zz,:)];
rhat1=[rhat1;rhat1(zz,:);rhat1(zz,:)];
rhat2=[rhat2;rhat2(zz,:);rhat2(zz,:)];
qhat1=[qhat1;qhat1(zz,:);qhat1(zz,:)];
qhat2=[qhat2;qhat2(zz,:);qhat2(zz,:)];
hhat=[hhat;hhat(zz,:);hhat(zz,:)];
end
nvec=length(T);index=[index;addi];% update set of indeces

cc=find(c==0);c(cc)=realmin;%avoids Nan error for velocities
%Set initial guesses of x
x=zeros(nvec,1);%initialize

Tp=4/3*(1-q.^3);%T parabola
jj=find(T<Tp);x(jj)=1.5;%initial guess on hyperbola
%ii=find(N==0);if ~isempty(ii);x(ii)=lamseries(T(ii)./Tp(ii)-1,c(ii),s(ii));end
ll=find(N>0);x(ll)=(.5+sign(Nrev(ll))*.5)./(2*N(ll)+1);%initial multirev guess

err=max(abs(Tx(x,q,N)-T)./Tx(x,q,N));%initialize convergence check variable

iteration=0;
while err>1e-6
       iteration=iteration+1;
       if iteration==50;disp('50 Lambert iterations');end
       [TX DTDx]=Tx(x,q,N);
       dT=TX-T;
       x=x-dT./DTDx;%x(i+1)=x(i)-DT/(DT/Dx)
       %catches found in MASL code
		 ii=find(x<-1);x(ii)=(x(ii)+dT(ii)./DTDx(ii)-2)/3;
		 ii=find(x>=1&N>0);x(ii)=(x(ii)+dT(ii)./DTDx(ii)+1)/2;
       err=max(abs(dT)./TX);
end

a=-s/2./(x.^2-1);
u1=sqrt(2*mu.*s).*(q.*sqrt(1+q.^2.*(x.^2-1)).*(s-mag(r1))-x.*(s-mag(r2)))./c./mag(r1);
u2=-sqrt(2*mu.*s).*(q.*sqrt(1+q.^2.*(x.^2-1)).*(s-mag(r2))-x.*(s-mag(r1)))./c./mag(r2);
v1=sqrt(mu.*(2./mag(r1)-1./a-u1.^2./mu));
v2=sqrt(mu.*(2./mag(r2)-1./a-u2.^2./mu));
V1=[u1 v1 zeros(nvec,1)];V2=[u2 v2 zeros(nvec,1)];
%Transform into X Y Z coords
%V1=V1*[rhat1' qhat1' hhat']';
%V2=V2*[rhat2' qhat2' hhat']';
cx1=[rhat1(:,1) qhat1(:,1) hhat(:,1)];cy1=[rhat1(:,2) qhat1(:,2) hhat(:,2)];cz1=[rhat1(:,3) qhat1(:,3) hhat(:,3)];
cx2=[rhat2(:,1) qhat2(:,1) hhat(:,1)];cy2=[rhat2(:,2) qhat2(:,2) hhat(:,2)];cz2=[rhat2(:,3) qhat2(:,3) hhat(:,3)];
V1=[dot(V1',cx1')' dot(V1',cy1')' dot(V1',cz1')'];
V2=[dot(V2',cx2')' dot(V2',cy2')' dot(V2',cz2')'];

%switch rows back to columns
if rowcol<0;V1=V1';V2=V2';a=a';N=N';iteration=iteration';index=index';end
return

%Lancaster's Algorithm
function [T,DTDx]=Tx(x,q,N)
nvec=length(x);index=[1:nvec]';
%initialize variables
T=zeros(nvec,1);DTDx=zeros(nvec,1);
y=zeros(nvec,1);z=zeros(nvec,1);f=zeros(nvec,1);g=zeros(nvec,1);d=zeros(nvec,1);

K=q.^2;
E=x.^2-1;
ii=find(abs(x-1)<1e-2);%if abs(x-1)<1e-2
       T(ii)=sigma(-E(ii))-q(ii).*K(ii).*sigma(-K(ii).*E(ii));%T=sigma(-E)-q*K*sigma(-K*E);
       DTDx(ii)=2*x(ii).*(q(ii).*K(ii).^2.*dsigma(-K(ii).*E(ii))-dsigma(-E(ii)));
jj=setdiff(index,ii );%else
       y(jj)=sqrt(abs(E(jj)));
       z(jj)=sqrt(1+K(jj).*E(jj));
       zz=find(z==0);z(zz)=realmin;
       f(jj)=y(jj).*(z(jj)-q(jj).*x(jj));
       g(jj)=x(jj).*z(jj)-q(jj).*E(jj);
       kk=jj(find(E(jj)<0));%if E<0
              d(kk)=N(kk)*pi+acos(g(kk)./sqrt(g(kk).^2+f(kk).^2));
       ll=setdiff(jj,kk );%else
              d(ll)=log(f(ll)+g(ll));
       %end
       T(jj)=2*(x(jj)-q(jj).*z(jj)-d(jj)./y(jj))./E(jj);
       DTDx(jj)=(4-4*q(jj).*K(jj).*x(jj)./z(jj)-3*x(jj).*T(jj))./E(jj);
%end

return

%Find if orbit with N revs is possible for given q & T
function yy=NorbT(q,T,N)
%yy is index of q that satisfies possibility of N revs    
zz=[1:length(q)]';
xx=[];
xN1=zeros(length(zz),1);
[TxN DTDxN1]=Tx(xN1,q,N);%min energy time (good initial guess)
aa=find(T<TxN);%N+1 rev possible for not aa
yy=setdiff(zz,zz(aa));zz=zz(aa);
xN2=.1*ones(length(zz),1);
xN1=xN1(aa);DTDxN1=DTDxN1(aa);
while ~isempty(zz)
	[TxN DTDxN2]=Tx(xN2,q(zz),N(zz));
	aa=find(T(zz)<TxN);%N+1 rev possible for not aa
	yy=union(setdiff(zz,zz(aa)),yy);zz=zz(aa);
	aa=find(DTDxN2(aa) > 1e-6);%more iteration on aa
	xx=union(setdiff(zz,zz(aa)),xx);zz=zz(aa);%no more on xx
	DTDxN3=DTDxN2(aa)-DTDxN1(aa);
	xN3=DTDxN2(aa).*xN1(aa)-DTDxN1(aa).*xN2(aa);
	%Don't fully understand significance of next line
	ww=find(xN3/1e38>DTDxN3);if ~isempty(ww);disp('INF ERROR');break;return;end
	xN3=xN3./DTDxN3;%next x
	xN1=xN1(aa);xN2=xN2(aa);
	DTDxN1=DTDxN1(aa);DTDxN2=DTDxN2(aa);
	%Check found in MASL code
	vv=find( abs(xN2-xN3)<abs(xN1-xN3) );
		xN1(vv)=xN2(vv);
		DTDxN1(vv)=DTDxN2(vv);
	%end
	xN2=xN3;
end
return

function out=sigma(u)
out=4/3+2/5*u+3/14*u.^2+5/36*u.^3+35/352*u.^4+945/12480*u.^5;
return

function out=dsigma(u)
out=2/5+3/7*u+5/12*u.^2+35/88*u.^3+945/2496*u.^4+10395/28800*u.^5;
return

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