function [dis az1 az2 iter_num k k1 bet2 bet1 sigm]= GeodesicInverseV2(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 and vertex's latitude
%   *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.
 %defaultellipsoid
%if nargin < 5, Geod=[6378137 0.0818191908426215]; end
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 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);
%ew(~s)=longitude12(bet1(~s),bet2(~s),cosd(bet1(~s)),0,e);
%ew(~s)=lambda12(bet1(~s),bet2(~s),90,0,e)
ew(~s)=longitude12(bet1(~s),bet2(~s),cosd(bet1(~s)),0,e);
ns=lon12-ew;

s1=(bet1==0) & ns<=0;
s2=(lon12==0) | (lon12==180);
s=s1 | s2;

c=bet1*0;
iter_num=c;
iter_num1=c;
c(s1)=1;
c(s2)=0;

 alp1=asind(c./cosd(bet1));
 alp2=alp1;
if any(~s) 
   c(~s)=InitialValue(bet1(~s),bet2(~s),lon12(~s),ns(~s),e);
   %c(~s)=cosd(bet1)-1*eps;
   %c(~s)=1*eps;
   alp1(~s)=asind(c(~s)./cosd(bet1(~s)));
   bv(~s)=acosd(c(~s));
   [c(~s) iter_num(~s) k]=NewtonMethod(bet1(~s),bet2(~s),lon12(~s),ns(~s),c(~s),e);     
    [bv(~s) iter_num(~s) k]=NewtonMethod2(bet1(~s),bet2(~s),lon12(~s),ns(~s),bv(~s),e); 
   %alp1(~s)=asind(c(~s)./cosd(bet1(~s)))
  %alp1(~s)=90-2^6*eps
  % alp1(~s)=  89.9996370868158-1000000*eps%+2^2*2^7*eps
  
   [alp1(~s) iter_num1(~s) k1]=NewtonMethod1(bet1(~s),bet2(~s),lon12(~s),ns(~s),alp1(~s),e);
   
   %[c(~s) sind(alp1(~s)).*cosd(bet1(~s)) alp1(~s)]
end


PV=ns(~s1)>0; 

%bv=acosd(cc);
%t1=-2*bv.*PV+bet1(~s1).*(-1).^PV;
%t2=bet2(~s1);
dis=c*0;

dis1=dis;
if any(~s1)
   %dis2=LongGeodesicDist(t2,c(~s),Geod)
   %dis1=LongGeodesicDist(t1,c(~s),Geod)
   %dis(~s)=dis2-dis1;
    [dis(~s1) sigm]=distance12(bet1(~s1),bet2(~s1),c(~s1),PV,Geod); 
    %alp1(~s1)=3.56406860995577
    [dis1(~s1) alp2(~s1) sigm]=distance12a(bet1(~s1),bet2(~s1),alp1(~s1),PV,Geod); 
end
dis(s1)=a*lon12(s1)/180*pi;

PV=ns>0;
az1=asind(c./cosd(bet1));
az2=asind(c./cosd(bet2));
az1=alp1;
az2=alp2;

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)
   %w12=lon12(~PV)/sqrt(1-e^2*((cosd(bet1)+cosd(bet2))/2)^2);
   ew=lon12(~PV)-ns(~PV);
   w12=lon12(~PV)/sqrt(1-e^2);
   bet11=bet1(~PV);
   bet21=bet2(~PV);  
   c(~PV)=sind(w12)./sqrt((tand(bet11).^2+tand(bet21).^2-2*tand(bet11).*tand(bet21).*cosd(w12)+sind(w12).^2));
   
   s=w12>ew; 
      
   cc=c(~PV);
   cc(s)=cosd(bet11(s))-eps;
   c(~PV )=cc;
   
   

end


%AR=180-2*rad2deg(GeodesicLat2Lon(-bet1,cosd(bet1),e));
AR=180-longitude12(bet1,-bet1,cosd(bet1),0,e);
s=(lon12+AR)>=179; %200; %179; 
s=s & abs(bet1+bet2)<=(cosd(bet1).*AR);
s=s& PV;
%s1=power(bet1+bet2,2).^(1/3)+power((180-lon12).*cosd(bet1),2).^(1/3)
%f=1-sqrt(1-e^2);
%s=s1<power(f*180*cosd(bet1)^2,2)^(1/3)
%[abs(bet1+bet2) cosd(bet1) AR bet1]
if any(s)
    c(s)=AntipodalRegion(bet1(s),bet2(s),lon12(s),e);    
end

%c(PV)=AntipodalRegion(bet1(PV),bet2(PV),lon12(PV),e);
%[c(s) c(PV)]
s=(~s & PV);

if any(s)
    
   w12=lon12(s);
   %w12=lon12(s)/sqrt(1-e^2)
   bet11=bet1(s);
   bet21=bet2(s);
   c(s)=sind(w12)./sqrt((tand(bet11).^2+tand(bet21).^2-2*tand(bet11).*tand(bet21).*cosd(w12)+sind(w12).^2));
   %ew=rad2deg(GeodesicLat2Lon(bet2(s),abs(cosd(bet1(s))),0));
   %ew=ew-rad2deg(GeodesicLat2Lon(bet1(s),abs(cosd(bet1(s))),0));
   
   ew=longitude12(bet1(s),bet2(s),cosd(bet1(s)),0,e);
   %Check the wrong initial value whether is not in domain range
   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];
c=bet1*0; 
fx=c*0+1;

for i=1:10
    cont=abs(fx)>1e-8;   
    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);
    dfx=(4*p1(:,1).*cc.^3+3*p1(:,2).*cc.^2+2*p1(:,3).*cc+ p1(:,4));
    c(cont)=cc-fx(cont)./dfx;    
end

    ind=db<1e-15;
    c(ind)=2*AR(ind)/e^2/180;
    
    %c(abs(ns)<1e-15)=cosd(bet1(abs(ns)<1e-15));

end

