BeginPackage["Vincenty`"];

VincentyDistance::usage="VincentyDistance[{lat1, lon1},{lat2, lon2}] computes the distance between two points with the given latitudes and longitudes."

Begin["`Private`"];

SetAttributes[ArcTanOrAngle, Listable];
ArcTanOrAngle[tan_, \[Phi]_] := 
    If[Abs[\[Phi]] < \[Pi]/2, ArcTan[tan], \[Phi]];

VincentyDistanceS[{\[Phi]1_, \[Theta]1_},{\[Phi]1_, \[Theta]1_}]  := 
    {0., {0.,0.}};

VincentyDistanceS[{\[Phi]1_, \[Theta]1_}, {\[Phi]2_, \[Theta]2_}] := 
  Module[{a0, flat, r, b0, tanu1, tanu2, dtmp, cosu1, sinu1, cosu2, 
      sinu2, \[Omega], cs, ss, cosalpha2, c2sm, u2, a, b, dsigma, s, alpha12, 
      alpha21},
    a0 = 6378137.00;
    flat = 1./298.257223563;
    r = 1. - flat;
    b0 = a0*r;
    
    tanu1 = r*Tan[\[Phi]1];
    tanu2 = r*Tan[\[Phi]2];
    
    dtmp = ArcTanOrAngle[tanu1, \[Phi]1];
    cosu1 = Cos[dtmp];
    sinu1 = Sin[dtmp];
    
    dtmp = ArcTanOrAngle[tanu2, \[Phi]2];
    cosu2 = Cos[dtmp];
    sinu2 = Sin[dtmp];
    
    \[Omega] = \[Theta]2 - \[Theta]1;
    
    itfun[\[Lambda]_] := Module[{ss1, ss2, tansigma, sinalpha, c},
        ss1 = cosu2*Sin[\[Lambda]];
        ss2 = cosu1*sinu2 - sinu1*cosu2*Cos[\[Lambda]];
        ss = Sqrt[ss1*ss1 + ss2*ss2];
        cs = sinu1*sinu2 + cosu1*cosu2*Cos[\[Lambda]];
        tansigma = ss/cs;
        sinalpha = cosu1*cosu2*Sin[\[Lambda]]/ss;
        cosalpha2 = Cos[ArcSin[sinalpha]]^2;
        c2sm = cs - 2.*sinu1*sinu2/cosalpha2;
        c = flat/16.*cosalpha2*(4. + flat*(4. - 3.*cosalpha2));
        \[Omega] + (1. - c)*flat*
            sinalpha*(ArcSin[ss] + c*ss*(c2sm + c*cs*(-1. + 2.*c2sm*c2sm)))];
    
    \[Lambda] = FixedPoint[itfun, \[Omega]];

    u2 = cosalpha2*(a0*a0 - b0*b0)/(b0*b0); 
    a = 1. + (u2/16384.)*(4096. + u2*(-768. + u2*(320. - 175.*u2))); 
    b = (u2/1024.)*(256. + u2*(-128. + u2*(74. - 47.*u2)));
    
    dsigma = 
      b*ss*(c2sm + (b/
                  4.)*(cs*(-1. + 2.*c2sm*c2sm) - (b/6.)*
                    c2sm*(-3. + 4.*ss*ss)*(-3. + 4.*c2sm*c2sm)));
    
    s = b0*a*(ArcSin[ss] - dsigma);
    
    alpha12 = 
      ArcTan[cosu1*sinu2 - sinu1*cosu2*Cos[\[Lambda]], cosu2*Sin[\[Lambda]]]; 
    alpha21 = 
      ArcTan[-sinu1*cosu2 + cosu1*sinu2*Cos[\[Lambda]], 
        cosu1*Sin[\[Lambda]]];
    {s, {alpha12, alpha21}}
];

ToRadians[deg_] := \[Pi] deg/180;

ToDegrees[rad_] := 180 rad/\[Pi];

VincentyDistance[{Lat1_, Long1_},{Lat2_, Long2_}] := 
Module[{s, az},
    {s, az} = 
      VincentyDistanceS[ToRadians[{Lat1, Long1}], ToRadians[{Lat2, Long2}]];
    {s, Mod[ToDegrees[az] + 180 {2,1},360]}];

End[];

EndPackage[];