function [dis az1 az2 t1 iter_num bet1 bet2 c]= GeodesicInversec(lat1,lon1,lat2,lon2,Geod)
%   [dis az1 az2] =  GeodesicInverseV1(lat1,lon1,lat2,lon2,Geod) computes 
%   the geodesic distance and azimuths assuming that the points lie on the 
%   reference ellipsoid defined by the input Geod.  
%   The input latitudes and longitudes, lat1, lon1, lat2, lon2, can
%   be scalars or arrays of equal size and must be expressed in degrees. 
%   The Geoid  vector is of the form Geod=[semimajor axis, eccentricity].  
%   The output dis (distance) is expressed in the same distance units as 
%   the semimajor axis of the ellipsoid vector. 
%   The output  azi1 and azi2 are the initial and final azimuths at the two endpoints in
%   degrees. If ellipsoid is omitted, the  WGS84 Geod=[6378137 0.0818191908426215] is used. 
%   This function duplicates some of the functionality of the DISTANCE
%   function in the MATLAB mapping toolbox and GEODDISTANCE function in Karney's geographiclib.
%   The differences and equvalent abilities are
%   *The algorithm directly uses the drivative of longitude difference with
%    respect to the Clairnaut constant c 
%   *The solution is also acurrate for all distances and azimuthes between
%    two endpoints on spheroiddal model compared to Karney's  GEODDISTANCE function.
%   *The algorithm also converges for all pairs  of input points.

if nargin < 5, Geod=defaultellipsoid; end
e=Geod(2);
a=Geod(1);

lon12=wrapnpi2pi(lon2-lon1);
lonsign=lon12<0;
lon12=abs(lon12);

swaplat = (abs(lat1) < abs(lat2));
[lat1(swaplat), lat2(swaplat)]=swap(lat1(swaplat), lat2(swaplat));

latsign=(lat1<0);
lat2=sign(-lat1).*lat2;
lat1=-abs(lat1);

bet1=phi2beta(lat1,e);
bet2=phi2beta(lat2,e);

ew=bet1*0;
%calculate the longitude difference of the first intersection between parallel of
%latitude bet2 and geodesic from point A with vertex latitude bet1
s=(abs(bet1)<eps);
ew(s)=180*sqrt(1-e^2);

if any(~s)
%ew(~s)=longitude12(bet1(~s),bet2(~s),cosd(bet1(~s)),0,e);
ew(~s)=longitude12c(bet1(~s),bet2(~s),cosd(bet1(~s)),0,e);
end
% determining whether the geodesic passes the vertex
ns=lon12-ew;

s1=(bet1==0) & ns<=0;
s2=(lon12==0) | (lon12==180);
s=s1 | s2;
Z=bet1*0;
c=Z;
iter_num=Z;

c(s1)=1;
c(s2)=0;
 t1=0;
if any(~s) 
   %c(~s)=InitialValue(bet1(~s),bet2(~s),lon12(~s),ns(~s),e); 
   %tic
   %[c(~s) iter_num(~s) k]=NewtonMethod(bet1(~s),bet2(~s),lon12(~s),ns(~s),c(~s),e); 
   %t1=toc
   c(~s)=InitialValue(bet1(~s),bet2(~s),lon12(~s),ns(~s),e); 
   %c(~s)=cosd(bet1(~s))-eps;
   %c(~s)=eps;
   %tic
   
   [c(~s) iter_num(~s) t1]=NewtonMethodc(bet1(~s),bet2(~s),lon12(~s),ns(~s),c(~s),e); 
   %t1=toc;
   
   %[c(~s) iter_num(~s) k]=vincenty(bet1(~s),bet2(~s),lon12(~s),ns(~s),c(~s),e); 
end

PV=ns(~s1)>0; 
dis=Z;

if any(~s1)
   dis(~s1)=distance12(bet1(~s1),bet2(~s1),c(~s1),PV,Geod);   
end
dis(s1)=a*lon12(s1)/180*pi;

PV=ns>0;
az1=asind(c./cosd(bet1));
az2=asind(c./cosd(bet2));
flagY=(-1).^lonsign;
flagX2=xor(~latsign,swaplat);
flagX1=xor(PV,flagX2);
az1=(180*flagX1+(-1).^flagX1.*az1).*flagY;
az2=(180*flagX2+(-1).^flagX2.*az2).*flagY;
[az1(swaplat), az2(swaplat)]=swap(az1(swaplat),az2(swaplat));


end


function c=InitialValue(bet1,bet2,lon12,ns,e)
c=ns*0;
PV=(ns>0); %Pass Vertex

if any(~PV)     
   bet11=bet1(~PV);
   bet21=bet2(~PV);
   %w12=lon12(~PV)/sqrt(1-e^2);
  %w12=lon12(~PV)./sqrt(1-e^2*((cosd(bet11)+cosd(bet21))/2).^2);
 %  w12=lon12(~PV)./sqrt(1-e^2*cosd(bet21).^2);
  %w12=lon12(~PV)./sqrt(1-e^2*cosd(bet11).^2);
 w12=lon12(~PV)./sqrt(1-e^2*(cosd(bet11).^2+cosd(bet21).^2)/2);
 
   c(~PV)=sind(w12)./sqrt((tand(bet11).^2+tand(bet21).^2-2*tand(bet11).*tand(bet21).*cosd(w12)+sind(w12).^2));
   
   ew=lon12(~PV)-ns(~PV);
   s=w12>ew;      
   cc=c(~PV);
   cc(s)=cosd(bet11(s))-eps;
   c(~PV )=cc;     
end


AR=180-longitude12(bet1,-bet1,cosd(bet1),0,e);
s=(lon12+AR)>=179;  
s=s & abs(bet1+bet2)<=(cosd(bet1).*AR);
s=s& PV;
if any(s)
    c(s)=AntipodalRegion(bet1(s),bet2(s),lon12(s),e);    
end

s=(~s & PV);

if any(s)   
   bet11=bet1(s);
   bet21=bet2(s);
   
   %w12=lon12(s).*sqrt(1-e^2*cosd(bet11).^2);
   %w12=lon12(s).*sqrt(1-e^2*cosd(bet21).^2);
   %w12=(w12+w21)/2
   %w12=lon12(s)./sqrt(1-e^2*((cosd(bet11)+cosd(bet21))/2).^2);
   %w12=lon12(s).*sqrt(1-e^2*(cosd(bet11).^2+cosd(bet21).^2)/2);
   %w12=lon12(s).*sqrt(1-e^2*(cosd((bet11+bet21))));
   w12=lon12(s);
   c(s)=sind(w12)./sqrt((tand(bet11).^2+tand(bet21).^2-2*tand(bet11).*tand(bet21).*cosd(w12)+sind(w12).^2));
   ew=longitude12(bet1(s),bet2(s),cosd(bet1(s)),0,e);
   s1=w12<ew;
   
   cc=c(s);   
   %If the value is overshot, the initial value is set to value of cosine of bigger latitude
   cc(s1)=cosd(bet11(s1))-eps;
   c(s)=cc;
end

end
function c=AntipodalRegion(bet1,bet2,lon12,e)

e2=e^2;
e4=e2*e2;
b1=bet1;
db=abs(bet2+bet1);
AR=180-lon12;
p=[cosd(b1).^2*e4*180^2,-4*cosd(b1).^2.*AR*e2*180,...
   4*db.^2-cosd(b1).^4*e4*180^2+4*cosd(b1).^2.*AR.^2,...
   4*cosd(b1).^4.*AR*e2*180,-4*cosd(b1).^4.*AR.^2];

ew=longitude12(bet1,bet2,cosd(bet1),0,e);
c=cosd(bet1)-(180-ew-AR)./(180-ew).*cosd(bet1);
ind=db<.01;   
c(ind)=2*AR(ind)/e^2/180;   
c=min(abs(c),cosd(bet1)-eps);
%c=c*0
fx=c*0+1;
%numit=bet1*0;
cont=fx;
for i=0:20
    cont=cont & abs(fx)>1e-10; 
   %numit(cont)=i;
    
    if ~any(cont), break, end 
    cc=c(cont);
    p1=p(cont,:); 
    fx(cont)=p1(:,1).*cc.^4+p1(:,2).*cc.^3+p1(:,3).*cc.^2+ p1(:,4).*cc+p1(:,5);
    cont=cont & abs(fx)>1e-10; 
    if ~any(cont), break, end  
    p1=p(cont,:); 
    cc=c(cont);
    dfx=(4*p1(:,1).*cc.^3+3*p1(:,2).*cc.^2+2*p1(:,3).*cc+ p1(:,4));
    cc=cc-fx(cont)./dfx;
    %cc(cc>1)=eps;
    c(cont)=cc;    
end
%numit
%sum(numit)
%fx
%p*(c.^[4 3 2 1 0]')
%c1=c;
   num=size(bet1,1);       
     for i=1:num
         cc=roots(p(i,:));
         c(i)=real(cc(end));
     end
   % c2=c
    ind=db<0.001;    
    c(ind)=2*AR(ind)/e^2/180;
  %  [c1 c2 c]
  
end

