/*
Solve the direct and inverse geodesic problems accurately.

Copyright (c) Charles Karney (2013) <charles@karney.com> and licensed
under the MIT/X11 License.  For more information, see
http://geographiclib.sourceforge.net/

References:

   Charles F. F. Karney,
   Algorithms for geodesics, J. Geodesy 87, 43-55 (2013),
   http://dx.doi.org/10.1007/s00190-012-0578-z
   Addenda: http://geographiclib.sf.net/geod-addenda.html

This program solves the geodesic problem either using series expansions
(exact : false) or using elliptic integrals (exact : true).  Elliptic
integrals give reliably high accuracy (especially when f is large).
Note that the area calculation always uses the series expansion (I don't
know how to express the integrals in terms of elliptic integrals).

Before running this file, you need to compute and save the series
expansions by editing god.mac setting maxpow appropriately (near the end
of the file) and uncommenting the last line (to save the results).  If
you're testing the accuracy of the series expansions (exact : false) or
if you're interested in accurate results for the area, that pick a
largish value of maxpow (e.g., 20).  This program can truncate the
series to a smaller power.  If you just want to compute accurate
geodesics and are not interested in the area, then use elliptic
integrals (exact : true) and leave maxpow at some small value (6 or 8).

To use this program,

(1) Edit the file name for the series "geod30.lsp" to reflect the value
of maxpow that you used.

(2) Set fpprec (the number of decimal digits of precision).

(3) Set exact (true for elliptic integrals, false for series).

(4) If exact = false, set the order of the series you want to use, by
replacing the "20" in min(maxpow,20) below.

(5) Start maxima and run

  load("geodesic.mac")$

(If you want to change fpprec, exact, or the order of the series, you
should edit this file and run this command again.)

(6) Define an ellipsoid with

  g:geod_init(radius, flattening)$

The ellipsoids wgs84 and grs80 are pre-defined.

(7) To solve a direct problem, run

  geod_direct(ellipsoid, lat1, lon1, azi1, s12);

e.g.,

  geod_direct(wgs84, -30, 0, 45, 1b7);

This returns a list, [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12], e.g.,

[9.00979560785581153132573611946234278938679821643464129576496b1,
3.795350501490084914310911431201705948430953526031024848204b1,
6.3403810943391419431089434638892210208040664981080107562114b1,
5.09217379721155238753530133334186917347878103616352193700526b1,1.0b7,
6.35984161356437923135381788735707599997546833534230510111197b6,
-1.42475315175601879366432145888870774855600761387337970018946b-3,
-7.47724030796032158868881196081763293754486469000152919698785b-4,
4.18229766667689593851692491830627309580972454148317773382384b12]

(8) To solve an inverse problem, run

  geod_inverse(ellipsoid, lat1, lon1, lat2, lon2);

e.g.,

  geod_inverse(wgs84, -30, 0, 29.9b0, 179.9b0);

This returns a list, [a12, s12, azi1, azi2, m12, M12, M21, S12], e.g.,

[1.79898924051433853264945993266804037171884583041584874134816b2,
1.99920903023269266279365620501124020214744990997998731526732b7,
1.70994569965518052741917124376016361591705298855243347424863b2,
8.99634915141674951478756137150809390696858860117887233257945b0,
6.04691725017600149958466836698242794713940408239599801996017b4,
-9.95488849775559128989753386111595867497859420132749768254471b-1,
-1.00448979492598025351148808245250847420628601706577993586242b0,
-1.14721359300439474273586680489951630835335433189068889945966b14]

(9) Use geod_polygonarea(ellipsoid, points) to compute polygon areas.

(10) The interface is copied from the C library for geodesics which is
documented at

  http://geographiclib.sourceforge.net/html/C/index.html

*/

/* Load series created by geod.mac (NEED TO UNCOMMENT THE LAST LINE OF
geod.mac TO GENERATE THIS FILE). */
load("geod30.lsp")$

/* Edit to reflect precision and order of the series to be used */
( fpprec:60, exact:true,
  GEOGRAPHICLIB_GEODESIC_ORDER:if exact then maxpow else min(maxpow,20))$

if exact then load("ellint.mac")$

( nA1   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nC1   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nC1p  :GEOGRAPHICLIB_GEODESIC_ORDER,
  nA2   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nC2   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nA3   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nA3x  :nA3,
  nC3   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nC3x  :((nC3 * (nC3 - 1)) / 2),
  nC4   :GEOGRAPHICLIB_GEODESIC_ORDER,
  nC4x  :((nC4 * (nC4 + 1)) / 2) )$

taylordepth:5$
jtaylor(expr,var1,var2,ord):=expand(subst([zz=1],
    ratdisrep(taylor(subst([var1=zz*var1,var2=zz*var2],expr),zz,0,ord))))$
ataylor(expr,var,ord):=expand(ratdisrep(taylor(expr,var,0,ord)))$
if not exact then (
  A1m1f(eps):=''((horner(ataylor(A1*(1-eps)-1,eps,nA1))
      +eps)/(1-eps)),
  C1f(eps):=''(block([l:[]],for i:1 thru nC1 do
      l:endcons(horner(ataylor(C1[i],eps,nC1)),l),l)),
  C1pf(eps):=''(block([l:[]],for i:1 thru nC1p do
      l:endcons(horner(ataylor(C1p[i],eps,nC1p)),l),l)),
  A2m1f(eps):=''(horner(ataylor(A2/(1-eps)-1,eps,nA2))
    *(1-eps) - eps),
  C2f(eps):=''(block([l:[]],for i:1 thru nC2 do
      l:endcons(horner(ataylor(C2[i],eps,nC2)),l),l)),
  A3coeff(n):=
  ''(block([q:jtaylor(A3,n,eps,nA3-1),l:[]],
      for i:0 thru nA3-1 do l:endcons(horner(coeff(q,eps,i)),l),
      l)),
  C3coeff(n):=
  ''(block([q,l:[]],
      for m:1 thru nC3-1 do (
        q:jtaylor(C3[m],n,eps,nC3-1),
        for j:m thru nC3-1 do l:endcons(horner(coeff(q,eps,j)),l)),
      l)))$
C4coeff(n):=
''(block([q,l:[]],
    for m:0 thru nC4-1 do (
      q:jtaylor(C4[m],n,eps,nC4-1),
      for j:m thru nC4-1 do l:endcons(horner(coeff(q,eps,j)),l)),
    l))$

( digits:floor((fpprec-1)*log(10.0)/log(2.0)),
  epsilon : 0.5b0^(digits - 1),
  realmin : 0.1b0^(3*fpprec),
  pi : bfloat(%pi),
  maxit1 : 100,
  maxit2 : maxit1 + digits + 10,
  tiny : sqrt(realmin),
  tol0 : epsilon,
  tol1 : 200 * tol0,
  tol2 : sqrt(tol0),
  tolb : tol0 * tol2,
  xthresh : 1000 * tol2,
  degree : pi/180,
  NaN : 'nan )$

sq(x):=x^2$
/*
log1px(x):=block([y,z],
    y : 1 + x,
    z : y - 1,
    if z = 0b0 then x else x * log(y) / z)$

atanhx(x):=block([y:abs(x)],
  y : log1px(2 * y/(1 - y))/2,
  if x < 0b0 then -y else y)$
*/
atanhx(x):=atanh(x)$
hypotx(x, y):= sqrt(x * x + y * y)$
atan2x(y,x) := -atan2(-y, x)$  /* fix atan2(0b0,-1b0) = -pi */

/*
pow(x,y):=x^y$
cbrtx(x) := block([y:pow(abs(x), 1/3b0)],
  if x < 0b0 then -y else y)$
*/
cbrtx(x):=x^(1/3)$

sumx(u, v):=block([s,up,vpp,t],
  s : u + v,
  up : s - v,
  vpp : s - up,
  up : up-u,
  vpp : vpp-v,
  t : -(up + vpp),
  [s,t])$

swapx(x, y):=[y,x]$

SinCosNorm(sinx, cosx):=block([r : hypotx(sinx, cosx)],
  [sinx/r, cosx/r])$

AngNormalize(x):= if x >= 180b0 then x - 360b0
else (if x < -180b0 then x + 360b0 else x)$
AngNormalize2(x):=AngNormalize(mod(x, 360b0))$

AngDiff(x, y) := block([t,d,r:sumx(-x,y)],
  d:r[1], t:r[2],
  if (d - 180b0) + t > 0b0 then d : d - 360b0
  else if (d + 180b0) + t <= 0b0 then d : d + 360b0,
  d + t)$

AngRound(x) := block([z:1/16b0, y:abs(x)],
  y : if y < z then z - (z - y) else y,
  if x < 0b0 then -y else y)$

/* Indices in geodesic struct */
block([i:0], g_a:(i:i+1), g_f:(i:i+1), g_f1:(i:i+1), g_e2:(i:i+1),
  g_ep2:(i:i+1), g_n:(i:i+1), g_b:(i:i+1), g_c2:(i:i+1), g_etol2:(i:i+1),
  g_A3x:(i:i+1), g_C3x:(i:i+1), g_C4x:(i:i+1) )$
geod_init(a, f):= (a:bfloat(a),f:bfloat(f),
  block([f1,e2,ep2,n,b,c2,etol2],
    f1:1-f, e2:f*(2-f), ep2:e2/f1^2, n:f/(2-f), b:a*f1,
    c2 : (sq(a) + sq(b) *
      (if e2 = 0b0 then 1b0 else
        (if e2 > 0b0 then atanhx(sqrt(e2)) else atan(sqrt(-e2))) /
        sqrt(abs(e2))))/2, /* authalic radius squared */
    /* The sig12 threshold for "really short".  Using the auxiliary sphere
    solution with dnm computed at (bet1 + bet2) / 2, the relative error in
    the azimuth consistency check is sig12^2 * abs(f) * min(1, 1-f/2) / 2.
    (Error measured for 1/100 < b/a < 100 and abs(f) >= 1/1000.  For a
    given f and sig12, the max error occurs for lines near the pole.  If
    the old rule for computing dnm = (dn1 + dn2)/2 is used, then the error
    increases by a factor of 2.)  Setting this equal to epsilon gives
    sig12 = etol2.  Here 0.1 is a safety factor (error decreased by 100)
    and max(0.001, abs(f)) stops etol2 getting too large in the nearly
    spherical case. */
    etol2 : 0.1b0 * tol2 / sqrt( max(0.001b0, abs(f)) * min(1b0, 1-f/2) / 2 ),
    [ a, f, f1, e2,
    ep2, n, b, c2, etol2,
    if exact then [] else bfloat(A3coeff(n)),
    if exact then [] else bfloat(C3coeff(n)),
    bfloat(C4coeff(n))]))$

/* Indices into geodesicline struct */
block([i:0],
  l_lat1:(i:i+1), l_lon1:(i:i+1), l_azi1:(i:i+1), l_a:(i:i+1), l_f:(i:i+1),
  l_b:(i:i+1), l_c2:(i:i+1), l_f1:(i:i+1), l_salp0:(i:i+1), l_calp0:(i:i+1),
  l_k2:(i:i+1), l_salp1:(i:i+1), l_calp1:(i:i+1),
  l_ssig1:(i:i+1), l_csig1:(i:i+1), l_dn1:(i:i+1),
  l_stau1:(i:i+1), l_ctau1:(i:i+1), l_somg1:(i:i+1), l_comg1:(i:i+1),
  if exact then (l_e2:(i:i+1), l_cchi1:(i:i+1), l_A4:(i:i+1), l_B41:(i:i+1),
    l_E0:(i:i+1), l_D0:(i:i+1), l_H0:(i:i+1),
    l_E1:(i:i+1), l_D1:(i:i+1), l_H1:(i:i+1),
    l_C4a:(i:i+1), l_E:(i:i+1),
    e_k2:1, e_alpha2:2, e_ec:3, e_dc:4, e_hc:5)
  else (l_A1m1:(i:i+1), l_A2m1:(i:i+1), l_A3c:(i:i+1),
    l_B11:(i:i+1), l_B21:(i:i+1), l_B31:(i:i+1),
    l_A4:(i:i+1), l_B41:(i:i+1), l_C1a:(i:i+1), l_C1pa:(i:i+1),
    l_C2a:(i:i+1), l_C3a:(i:i+1),
    l_C4a:(i:i+1) ))$

Ef(k2, alpha2):=if exact then [k2, alpha2, ec(k2), dc(k2), hc(k2, alpha2)]
else []$

geod_lineinit(g,lat1,lon1,azi1):=block([a, f,
  b, c2, f1, salp0, calp0,
  k2, salp1, calp1,
  ssig1, csig1, dn1,
  stau1, ctau1, somg1, comg1,
  A1m1, A2m1, A3c, B11, B21, B31,
  A4, B41, C1a, C1pa, C2a, C3a,
  C4a,
  alp1, cbet1, sbet1, phi, eps,
  e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E],
  lat1:bfloat(lat1),lon1:bfloat(lon1), azi1:bfloat(azi1),
  a : g[g_a],
  f : g[g_f],
  b : g[g_b],
  c2 : g[g_c2],
  f1 : g[g_f1],
  e2 : g[g_e2],
  /* If caps is 0 assume the standard direct calculation
  caps = (caps ? caps : GEOD_DISTANCE_IN | GEOD_LONGITUDE) |
  GEOD_LATITUDE | GEOD_AZIMUTH, Always allow latitude and azimuth
  Guard against underflow in salp0 */
  azi1 : AngRound(AngNormalize(azi1)),
  lon1 : AngNormalize(lon1),
  /* alp1 is in [0, pi] */
  alp1 : azi1 * degree,
  /* Enforce sin(pi) == 0 and cos(pi/2) == 0.  Better to face the ensuing
  problems directly than to skirt them. */
  salp1 : if azi1  = -180b0 then 0b0 else sin(alp1),
  calp1 : if abs(azi1) = 90b0 then 0b0 else cos(alp1),
  phi : lat1 * degree,
  /* Ensure cbet1 = +epsilon at poles */
  sbet1 : f1 * sin(phi),
  cbet1 : if abs(lat1) = 90b0 then tiny else cos(phi),
  block([t:SinCosNorm(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]),
  dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)),
  /* Evaluate alp0 from sin(alp1) * cos(bet1) = sin(alp0), */
  salp0 : salp1 * cbet1, /* alp0 in [0, pi/2 - |bet1|] */
  /* Alt: calp0 : hypot(sbet1, calp1 * cbet1).  The following
  is slightly better (consider the case salp1 = 0). */
  calp0 : hypotx(calp1, salp1 * sbet1),
  /* Evaluate sig with tan(bet1) = tan(sig1) * cos(alp1).
  sig = 0 is nearest northward crossing of equator.
  With bet1 = 0, alp1 = pi/2, we have sig1 = 0 (equatorial line).
  With bet1 =  pi/2, alp1 = -pi, sig1 =  pi/2
  With bet1 = -pi/2, alp1 =  0 , sig1 = -pi/2
  Evaluate omg1 with tan(omg1) = sin(alp0) * tan(sig1).
  With alp0 in (0, pi/2], quadrants for sig and omg coincide.
  No atan2(0,0) ambiguity at poles since cbet1 = +epsilon.
  With alp0 = 0, omg1 = 0 for alp1 = 0, omg1 = pi for alp1 = pi. */
  ssig1 : sbet1, somg1 : salp0 * sbet1,
  csig1 : comg1 : if sbet1 # 0b0 or calp1 # 0b0 then cbet1 * calp1 else 1b0,
  /* Without normalization we have schi1 = somg1. */
  cchi1 : f1 * dn1 * comg1,
  /* sig1 in (-pi, pi] */
  block([t:SinCosNorm(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
  /* SinCosNorm (somg1, comg1); -- don't need to normalize!
  SinCosNorm (schi1, cchi1); -- don't need to normalize! */
  k2 : sq(calp0) * g[g_ep2],
  eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
  E:Ef(-k2,-g[g_ep2]),
  block([s,c],
    if exact then (E0 : E[e_ec]/(pi/2),
      E1 : deltae(ssig1,csig1,dn1,E[e_k2],E[e_ec]),
      s:sin(E1),c:cos(E1))
    else ( A1m1 : A1m1f(eps),
      C1a : C1f(eps),
      B11 : SinCosSeries(true, ssig1, csig1, C1a),
      s : sin(B11), c : cos(B11)),
    /* tau1 = sig1 + B11 */
    stau1 : ssig1 * c + csig1 * s,
    ctau1 : csig1 * c - ssig1 * s
    /* Not necessary because C1pa reverts C1a
    B11 = -SinCosSeries(true, stau1, ctau1, C1pa, nC1p) */
    ),
  if exact then (D0 : E[e_dc]/(pi/2),
    D1 : deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc]),
    H0 : E[e_hc]/(pi/2),
    H1 : deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc]))
  else ( C1pa: C1pf(eps),
    A2m1 : A2m1f(eps),
    C2a : C2f(eps),
    B21 : SinCosSeries(true, ssig1, csig1, C2a),
    C3a : C3f(g, eps),
    A3c : -f * salp0 * A3f(g, eps),
    B31 : SinCosSeries(true, ssig1, csig1, C3a)),
  C4a : C4f(g, eps),
  /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0) */
  A4 : sq(a) * calp0 * salp0 * e2,
  B41 : SinCosSeries(false, ssig1, csig1, C4a),
  if exact then
    [ lat1, lon1, azi1, a, f,
    b, c2, f1, salp0, calp0,
    k2, salp1, calp1,
    ssig1, csig1, dn1,
    stau1, ctau1, somg1, comg1,
    e2, cchi1, A4, B41, E0, D0, H0, E1, D1, H1, C4a, E]
  else
    [ lat1, lon1, azi1, a, f,
    b, c2, f1, salp0, calp0,
    k2, salp1, calp1,
    ssig1, csig1, dn1,
    stau1, ctau1, somg1, comg1,
    A1m1, A2m1, A3c, B11, B21, B31,
    A4, B41, C1a, C1pa, C2a, C3a,
    C4a] )$

/* Return [a12, lat2, lon2, azi2, s12, m12, M12, M21, S12] */
geod_genposition(l, arcmode,  s12_a12):=block(
  [ lat2 : 0, lon2 : 0, azi2 : 0, s12 : 0,
  m12 : 0, M12 : 0, M21 : 0, S12 : 0,
  /* Avoid warning about uninitialized B12. */
  sig12, ssig12, csig12, B12 : 0, E2 : 0, AB1 : 0,
  omg12, lam12, lon12,
  ssig2, csig2, sbet2, cbet2, somg2, comg2, salp2, calp2, dn2],
  s12_a12 : bfloat(s12_a12),
  if (arcmode) then block([s12a],
    /* Interpret s12_a12 as spherical arc length */
    sig12 : s12_a12 * degree,
    s12a : abs(s12_a12),
    s12a : s12a - 180 * floor(s12a / 180),
    ssig12 : if s12a =  0b0 then 0b0 else sin(sig12),
    csig12 : if s12a = 90b0 then 0b0 else cos(sig12))
  else block([tau12 : s12_a12 / (l[l_b] *
      (if exact then l[l_E0] else (1 + l[l_A1m1]))),s,c],
    /* Interpret s12_a12 as distance */
    s : sin(tau12),
    c : cos(tau12),
    /* tau2 = tau1 + tau12 */
    if exact then (E2 : - deltaeinv(l[l_stau1] * c + l[l_ctau1] * s,
        l[l_ctau1] * c - l[l_stau1] * s,
        l[l_E][e_k2], l[l_E][e_ec]),
      sig12 : tau12 - (E2 - l[l_E1]), ssig12 : sin(sig12), csig12 : cos(sig12))
    else (B12 : - SinCosSeries(true,
        l[l_stau1] * c + l[l_ctau1] * s,
        l[l_ctau1] * c - l[l_stau1] * s,
        l[l_C1pa]),
      sig12 : tau12 - (B12 - l[l_B11]),
      ssig12 : sin(sig12), csig12 : cos(sig12),
      if abs(l[l_f]) > 0.01 then block(
        /* Reverted distance series is inaccurate for |f| > 1/100, so correct
        sig12 with 1 Newton iteration.  The following table shows the
        approximate maximum error for a = WGS_a() and various f relative to
        GeodesicExact.
            erri = the error in the inverse solution (nm)
            errd = the error in the direct solution (series only) (nm)
            errda = the error in the direct solution (series + 1 Newton) (nm)
              f     erri  errd errda
            -1/5    12e6 1.2e9  69e6
            -1/10  123e3  12e6 765e3
            -1/20   1110 108e3  7155
            -1/50  18.63 200.9 27.12
            -1/100 18.63 23.78 23.37
            -1/150 18.63 21.05 20.26
             1/150 22.35 24.73 25.83
             1/100 22.35 25.03 25.31
             1/50  29.80 231.9 30.44
             1/20   5376 146e3  10e3
             1/10  829e3  22e6 1.5e6
             1/5   157e6 3.8e9 280e6
        */
        [ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12,
        csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12,
        serr],
        B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]),
        serr : (1 + l[l_A1m1]) * (sig12 + (B12 - l[l_B11])) - s12_a12 / l[l_b],
        sig12 : sig12 - serr / sqrt(1 + l[l_k2] * sq(ssig2)),
        ssig12 : sin(sig12), csig12 : cos(sig12)
        /* Update B12 below */
        ))),
  /* sig2 = sig1 + sig12 */
  ssig2 : l[l_ssig1] * csig12 + l[l_csig1] * ssig12,
  csig2 : l[l_csig1] * csig12 - l[l_ssig1] * ssig12,
  dn2 : sqrt(1 + l[l_k2] * sq(ssig2)),
  if exact then (if arcmode
    then E2 : deltae(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_ec]),
    AB1 : l[l_E0] * (E2 - l[l_E1]))
  else (if arcmode or abs(l[l_f]) > 0.01b0
    then B12 : SinCosSeries(true, ssig2, csig2, l[l_C1a]),
    AB1 : (1 + l[l_A1m1]) * (B12 - l[l_B11])),
  /* sin(bet2) = cos(alp0) * sin(sig2) */
  sbet2 : l[l_calp0] * ssig2,
  /* Alt: cbet2 = hypot(csig2, salp0 * ssig2); */
  cbet2 : hypotx(l[l_salp0], l[l_calp0] * csig2),
  if cbet2 = 0b0 then
  /* I.e., salp0 = 0, csig2 = 0.  Break the degeneracy in this case */
  cbet2 : csig2 : tiny,
  if not exact then (
    /* tan(omg2) = sin(alp0) * tan(sig2) */
    somg2 : l[l_salp0] * ssig2, comg2 : csig2),  /* No need to normalize */
  /* tan(alp0) = cos(sig2)*tan(alp2) */
  salp2 : l[l_salp0], calp2 : l[l_calp0] * csig2, /* No need to normalize */
  if not exact then
  /* omg12 = omg2 - omg1 */
  omg12 : atan2x(somg2 * l[l_comg1] - comg2 * l[l_somg1],
    comg2 * l[l_comg1] + somg2 * l[l_somg1]),
  s12 : if arcmode then l[l_b] *
  ((if exact then l[l_E0] else (1 + l[l_A1m1])) * sig12 + AB1) else s12_a12,
  if exact then block([somg2:l[l_salp0] * ssig2,
    comg2 : csig2, /* No need to normalize */
    cchi2],
    /* Without normalization we have schi2 = somg2. */
    cchi2 : l[l_f1] * dn2 *  comg2,
    lam12 : atan2x(somg2 * l[l_cchi1] - cchi2 * l[l_somg1],
      cchi2 * l[l_cchi1] + somg2 * l[l_somg1]) -
    l[l_e2]/l[l_f1] * l[l_salp0] * l[l_H0] *
    (sig12 + deltah(ssig2, csig2, dn2,
        l[l_E][e_k2], l[l_E][e_alpha2], l[l_E][e_hc]) - l[l_H1] ) )
  else lam12 : omg12 + l[l_A3c] *
  ( sig12 + (SinCosSeries(true, ssig2, csig2, l[l_C3a]) - l[l_B31])),
  lon12 : lam12 / degree,
  /* Use AngNormalize2 because longitude might have wrapped multiple times. */
  lon12 : AngNormalize2(lon12),
  lon2 : AngNormalize(l[l_lon1] + lon12),
  lat2 : atan2x(sbet2, l[l_f1] * cbet2) / degree,
  /* minus signs give range [-180, 180). 0- converts -0 to +0. */
  azi2 : 0 - atan2x(-salp2, calp2) / degree,
  block([B22, AB2, J12],
    if exact then J12 : l[l_k2] * l[l_D0] *
    (sig12 + deltad(ssig2, csig2, dn2, l[l_E][e_k2], l[l_E][e_dc]) - l[l_D1])
    else ( B22 : SinCosSeries(true, ssig2, csig2, l[l_C2a]),
      AB2 : (1 + l[l_A2m1]) * (B22 - l[l_B21]),
      J12 : (l[l_A1m1] - l[l_A2m1]) * sig12 + (AB1 - AB2)),
    /* Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure
    accurate cancellation in the case of coincident points. */
    m12 : l[l_b] * ((dn2 * (l[l_csig1] * ssig2) -
        l[l_dn1] * (l[l_ssig1] * csig2))
      - l[l_csig1] * csig2 * J12),
    block([t : l[l_k2] * (ssig2 - l[l_ssig1]) *
      (ssig2 + l[l_ssig1]) / (l[l_dn1] + dn2)],
      M12 : csig12 + (t *  ssig2 -  csig2 * J12) * l[l_ssig1] / l[l_dn1],
      M21 : csig12 - (t * l[l_ssig1] - l[l_csig1] * J12) *  ssig2 /  dn2)),
  block([ B42 : SinCosSeries(false, ssig2, csig2, l[l_C4a]), salp12, calp12],
    if l[l_calp0] = 0b0 or l[l_salp0] = 0b0 then (
      /* alp12 = alp2 - alp1, used in atan2 so no need to normalized */
      salp12 : salp2 * l[l_calp1] - calp2 * l[l_salp1],
      calp12 : calp2 * l[l_calp1] + salp2 * l[l_salp1],
      /* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
      salp12 = -0 and alp12 = -180.  However this depends on the sign being
      attached to 0 correctly.  The following ensures the correct
      behavior. */
      if salp12 = 0b0 and calp12 < 0b0 then (
        salp12 : tiny * l[l_calp1],
        calp12 : -1))
    else (
      /* tan(alp) = tan(alp0) * sec(sig)
      tan(alp2-alp1) = (tan(alp2) -tan(alp1)) / (tan(alp2)*tan(alp1)+1)
      = calp0 * salp0 * (csig1-csig2) / (salp0^2 + calp0^2 * csig1*csig2)
      If csig12 > 0, write
        csig1 - csig2 = ssig12 * (csig1 * ssig12 / (1 + csig12) + ssig1)
      else
        csig1 - csig2 = csig1 * (1 - csig12) + ssig12 * ssig1
      No need to normalize */
      salp12 : l[l_calp0] * l[l_salp0] *
      ( if csig12 <= 0b0 then l[l_csig1] * (1 - csig12) + ssig12 * l[l_ssig1]
        else ssig12 * (l[l_csig1] * ssig12 / (1 + csig12) + l[l_ssig1])),
      calp12 : sq(l[l_salp0]) + sq(l[l_calp0]) * l[l_csig1] * csig2),
    S12 : l[l_c2] * atan2x(salp12, calp12) + l[l_A4] * (B42 - l[l_B41])),
  [if arcmode then s12_a12 else sig12 / degree,
  lat2, lon2, azi2, s12, m12, M12, M21, S12])$

geod_position(l, s12) := geod_genposition(l, false, s12)$

geod_gendirect(g, lat1, lon1, azi1, arcmode, s12_a12):=
block([l:geod_lineinit(g, lat1, lon1, azi1)],
  geod_genposition(l, arcmode, s12_a12))$

geod_direct(g, lat1, lon1, azi1, s12):=
  geod_gendirect(g, lat1, lon1, azi1, false, s12)$

/* Return [a12, s12, azi1, azi2, m12, M12, M21, S12] */
geod_geninverse(g, lat1, lon1, lat2, lon2):=block(
  [s12 : 0b0, azi1 : 0b0, azi2 : 0b0,
  m12 : 0b0, M12 : 0b0, M21 : 0b0, S12 : 0b0,
  lon12, latsign, lonsign, swapp,
  phi, sbet1, cbet1, sbet2, cbet2, s12x : 0b0, m12x : 0b0,
  dn1, dn2, lam12, slam12, clam12,
  a12 : 0b0, sig12, calp1 : 0b0, salp1 : 0b0, calp2 : 0b0, salp2 : 0b0,
  /* index zero elements of these arrays are unused */
  C1a, C2a, C3a,
  meridian,  omg12 : 0b0,
  /* Initialize for the meridian.  No longitude calculation is done in this
  case to let the parameter default to 0. */
  E : Ef(-g[g_ep2],  0b0)],
  lat1:bfloat(lat1),lon1:bfloat(lon1),
  lat2:bfloat(lat2),lon2:bfloat(lon2),
  /* Compute longitude difference (AngDiff does this carefully).  Result is
  in [-180, 180] but -180 is only for west-going geodesics.  180 is for
  east-going and meridional geodesics. */
  lon12 : AngDiff(AngNormalize(lon1), AngNormalize(lon2)),
  /* If very close to being on the same half-meridian, then make it so. */
  lon12 : AngRound(lon12),
  /* Make longitude difference positive. */
  lonsign : if lon12 >= 0b0 then 1 else -1,
  lon12 : lon12 * lonsign,
  /* If really close to the equator, treat as on equator. */
  lat1 : AngRound(lat1),
  lat2 : AngRound(lat2),
  /* Swap points so that point with higher (abs) latitude is point 1 */
  swapp : if abs(lat1) >= abs(lat2) then 1 else -1,
  if swapp < 0 then (lonsign : -lonsign,
    block([t:swapx(lat1, lat2)], lat1:t[1], lat2:t[2])),
  /* Make lat1 <= 0 */
  latsign : if lat1 < 0b0 then 1 else -1,
  lat1 : lat1 * latsign,
  lat2 : lat2 * latsign,
  /* Now we have
      0 <= lon12 <= 180
      -90 <= lat1 <= 0
      lat1 <= lat2 <= -lat1
  longsign, swapp, latsign register the transformation to bring the
  coordinates to this canonical form.  In all cases, 1 means no change was
  made.  We make these transformations so that there are few cases to
  check, e.g., on verifying quadrants in atan2.  In addition, this
  enforces some symmetries in the results returned. */
  phi : lat1 * degree,
  /* Ensure cbet1 = +epsilon at poles */
  sbet1 : g[g_f1] * sin(phi),
  cbet1 : if lat1 = -90b0 then tiny else cos(phi),
  block([t:SinCosNorm(sbet1, cbet1)], sbet1:t[1], cbet1:t[2]),
  phi : lat2 * degree,
  /* Ensure cbet2 = +epsilon at poles */
  sbet2 : g[g_f1] * sin(phi),
  cbet2 : if abs(lat2) = 90b0 then tiny else cos(phi),
  block([t:SinCosNorm(sbet2, cbet2)], sbet2:t[1], cbet2:t[2]),
  /* If cbet1 < -sbet1, then cbet2 - cbet1 is a sensitive measure of the
  |bet1| - |bet2|.  Alternatively (cbet1 >= -sbet1), abs(sbet2) + sbet1 is
  a better measure.  This logic is used in assigning calp2 in Lambda12.
  Sometimes these quantities vanish and in that case we force bet2 = +/-
  bet1 exactly.  An example where is is necessary is the inverse problem
  48.522876735459 0 -48.52287673545898293 179.599720456223079643
  which failed with Visual Studio 10 (Release and Debug) */
  if cbet1 < -sbet1 then
  ( if cbet2 = cbet1 then sbet2 : if sbet2 < 0b0 then sbet1 else -sbet1 )
  else (    if abs(sbet2) = -sbet1 then cbet2 : cbet1 ),
  dn1 : sqrt(1 + g[g_ep2] * sq(sbet1)),
  dn2 : sqrt(1 + g[g_ep2] * sq(sbet2)),
  lam12 : lon12 * degree,
  slam12 : if lon12 = 180b0 then 0b0 else sin(lam12),
  clam12 : cos(lam12),      /* lon12 == 90 isn't interesting */
  meridian : is(lat1 = -90b0 or slam12 = 0b0),
  if meridian then block(
    /* Endpoints are on a single full meridian, so the geodesic might lie on
    a meridian. */
    [ ssig1, csig1, ssig2, csig2],
    calp1 : clam12, salp1 : slam12, /* Head to the target longitude */
    calp2 : 1b0, salp2 : 0b0,           /* At the target we're heading north */
    /* tan(bet) : tan(sig) * cos(alp) */
    ssig1 : sbet1, csig1 : calp1 * cbet1,
    ssig2 : sbet2, csig2 : calp2 * cbet2,
    /* sig12 = sig2 - sig1 */
    sig12 : atan2x(max(csig1 * ssig2 - ssig1 * csig2, 0b0),
      csig1 * csig2 + ssig1 * ssig2),
    block([r],
      r:Lengths(g, g[g_n], E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
        cbet1, cbet2),
      s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]),
    /* Add the check for sig12 since zero length geodesics might yield m12 <
    0.  Test case was
       echo 20.001 0 20.001 0 | GeodTest -i
    In fact, we will have sig12 > pi/2 for meridional geodesic which is
    not a shortest path. */
    if sig12 < 1b0 or m12x >= 0b0 then (
      m12x : m12x * g[g_b],
      s12x : s12x * g[g_b],
      a12 : sig12 / degree )
    else
    /* m12 < 0, i.e., prolate and too close to anti-podal */
    meridian : false ),
  if not meridian and
  sbet1 = 0b0 and           /* and sbet2 == 0 */
  /* Mimic the way Lambda12 works with calp1 = 0 */
  (g[g_f] <= 0b0 or lam12 <= pi - g[g_f] * pi) then (
    /* Geodesic runs along equator */
    calp1 : calp2 : 0b0, salp1 : salp2 : 1b0,
    s12x : g[g_a] * lam12,
    sig12 : omg12 : lam12 / g[g_f1],
    m12x : g[g_b] * sin(sig12),
    M12 : M21 : cos(sig12),
    a12 : lon12 / g[g_f1] )
  else if not meridian then block([dnm],
    /* Now point1 and point2 belong within a hemisphere bounded by a
    meridian and geodesic is neither meridional or equatorial.
    Figure a starting point for Newton's method */
    block([r],
      r : InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, lam12),
      sig12:r[1], salp1:r[2], calp1:r[3], salp2:r[4], calp2:r[5],
      dnm:r[6]),
    if sig12 >= 0b0 then (
      /* Short lines (InverseStart sets salp2, calp2, dnm) */
      s12x : sig12 * g[g_b] * dnm,
      m12x : sq(dnm) * g[g_b] * sin(sig12 / dnm),
      M12 : M21 : cos(sig12 / dnm),
      a12 : sig12 / degree,
      omg12 : lam12 / (g[g_f1] * dnm))
    else block(
      /* Newton's method.  This is a straightforward solution of f(alp1) =
      lambda12(alp1) - lam12 = 0 with one wrinkle.  f(alp) has exactly one
      root in the interval (0, pi) and its derivative is positive at the
      root.  Thus f(alp) is positive for alp > alp1 and negative for alp <
      alp1.  During the course of the iteration, a range (alp1a, alp1b) is
      maintained which brackets the root and with each evaluation of
      f(alp) the range is shrunk, if possible.  Newton's method is
      restarted whenever the derivative of f is negative (because the new
      value of alp1 is then further from the solution) or if the new
      estimate of alp1 lies outside (0,pi); in this case, the new starting
      guess is taken to be (alp1a + alp1b) / 2. */
      [ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0,
      numit : 0,
      /* Bracketing range */
      salp1a : tiny, calp1a : 1b0, salp1b : tiny, calp1b : -1b0,
      tripn : false, tripb : false, contflag, dv, v],
      for i:0 thru maxit2-1 do (
        contflag:false,
        numit:i,
        /* the WGS84 test set: mean : 1.47, sd = 1.25, max = 16
        WGS84 and random input: mean = 2.85, sd = 0.60 */
        block([r],
          r : Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1,
            is(numit < maxit1)),
          v : r[1] - lam12,
          salp2:r[2], calp2:r[3], sig12:r[4],
          ssig1:r[5], csig1:r[6], ssig2:r[7], csig2:r[8],
          if exact then E:r[9] else eps:r[9], omg12:r[10], dv:r[11]),
        /* 2 * tol0 is approximately 1 ulp for a number in [0, pi]. */
        /* Reversed test to allow escape with NaNs */
        if tripb or not(abs(v) >= (if tripn then 8 else 2) * tol0) then
        return(true),
        /* Update bracketing values */
        if v > 0b0 and (numit > maxit1 or calp1/salp1 > calp1b/salp1b)
        then ( salp1b : salp1, calp1b : calp1 )
        else if v < 0b0 and (numit > maxit1 or calp1/salp1 < calp1a/salp1a)
        then ( salp1a : salp1, calp1a : calp1 ),
        if numit < maxit1 and dv > 0b0 then block(
          [dalp1, sdalp1, cdalp1,nsalp1],
          dalp1 : -v/dv,
          sdalp1 : sin(dalp1), cdalp1 : cos(dalp1),
          nsalp1 : salp1 * cdalp1 + calp1 * sdalp1,
          if nsalp1 > 0b0 and abs(dalp1) < pi then (
            calp1 : calp1 * cdalp1 - salp1 * sdalp1,
            salp1 : nsalp1,
            block([t:SinCosNorm(salp1, calp1)], salp1:t[1], calp1:t[2]),
            /* In some regimes we don't get quadratic convergence because
            slope -> 0.  So use convergence conditions based on epsilon
            instead of sqrt(epsilon). */
            tripn : is(abs(v) <= 16 * tol0),
            contflag:true ) ),
        if not contflag then (
          /* Either dv was not postive or updated value was outside legal
          range.  Use the midpoint of the bracket as the next estimate.
          This mechanism is not needed for the WGS84 ellipsoid, but it does
          catch problems with more eccentric ellipsoids.  Its efficacy is
          such for the WGS84 test set with the starting guess set to alp1 =
          90deg:
          the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
          WGS84 and random input: mean = 4.74, sd = 0.99 */
          salp1 : (salp1a + salp1b)/2,
          calp1 : (calp1a + calp1b)/2,
          block([t:SinCosNorm(salp1, calp1)], salp1:t[1], calp1:t[2]),
          tripn : false,
          tripb : is(abs(salp1a - salp1) + (calp1a - calp1) < tolb or
            abs(salp1 - salp1b) + (calp1 - calp1b) < tolb)) ),
      block([r],
        r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
          cbet1, cbet2),
        s12x:r[1], m12x:r[2], M12:r[4], M21:r[5]),
      m12x : m12x * g[g_b],
      s12x : s12x * g[g_b],
      a12 : sig12 / degree,
      if not exact then omg12 : lam12 - omg12 ) ),
  s12 : 0b0 + s12x,           /* Convert -0 to 0 */
  m12 : 0b0 + m12x,             /* Convert -0 to 0 */
  block(
    /* From Lambda12: sin(alp1) * cos(bet1) = sin(alp0) */
    [ salp0 : salp1 * cbet1,
    calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */
    alp12],
    if calp0 # 0b0 and salp0 # 0b0 then block(
      /* From Lambda12: tan(bet) = tan(sig) * cos(alp) */
      [ssig1 : sbet1, csig1 : calp1 * cbet1,
      ssig2 : sbet2, csig2 : calp2 * cbet2,
      k2 : sq(calp0) * g[g_ep2], eps,
      /* Multiplier = a^2 * e^2 * cos(alpha0) * sin(alpha0). */
      A4 : sq(g[g_a]) * calp0 * salp0 * g[g_e2],
      C4a, B41, B42],
      eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
      block([t:SinCosNorm(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
      block([t:SinCosNorm(ssig2, csig2)], ssig2:t[1], csig2:t[2]),
      C4a : C4f(g, eps),
      B41 : SinCosSeries(false, ssig1, csig1, C4a),
      B42 : SinCosSeries(false, ssig2, csig2, C4a),
      S12 : A4 * (B42 - B41))
    else S12 : 0, /* Avoid problems with indeterminate sig1, sig2 on equator */
    if not meridian and
    omg12 < 0.75b0 * pi and   /* Long difference too big */
    sbet2 - sbet1 < 1.75b0 then block( /* Lat difference too big */
      /* Use tan(Gamma/2) = tan(omg12/2)
      * (tan(bet1/2)+tan(bet2/2))/(1+tan(bet1/2)*tan(bet2/2))
      with tan(x/2) = sin(x)/(1+cos(x)) */
      [somg12 : sin(omg12), domg12 : 1 + cos(omg12),
      dbet1 : 1 + cbet1, dbet2 : 1 + cbet2],
      alp12 : 2 * atan2x( somg12 * ( sbet1 * dbet2 + sbet2 * dbet1 ),
        domg12 * ( sbet1 * sbet2 + dbet1 * dbet2 ) ))
    else block(
      /* alp12 = alp2 - alp1, used in atan2 so no need to normalize */
      [salp12 : salp2 * calp1 - calp2 * salp1,
      calp12 : calp2 * calp1 + salp2 * salp1],
      /* The right thing appears to happen if alp1 = +/-180 and alp2 = 0, viz
      salp12 = -0 and alp12 = -180.  However this depends on the sign
      being attached to 0 correctly.  The following ensures the correct
      behavior. */
      if salp12 = 0b0 and calp12 < 0b0 then (
        salp12 : tiny * calp1,
        calp12 : -1b0),
      alp12 : atan2x(salp12, calp12) ),
    S12 : S12 + g[g_c2] * alp12,
    S12 : S12 * swapp * lonsign * latsign,
    /* Convert -0 to 0 */
    S12 : S12 + 0b0 ),
  /* Convert calp, salp to azimuth accounting for lonsign, swapp, latsign. */
  if swapp < 0 then (
    block([t:swapx(salp1, salp2)], salp1:t[1], salp2:t[2]),
    block([t:swapx(calp1, calp2)], calp1:t[1], calp2:t[2]),
    block([t:swapx(M12, M21)], M12:t[1], M21:t[2])),
  salp1 : salp1 * swapp * lonsign, calp1 : calp1 * swapp * latsign,
  salp2 : salp2 * swapp * lonsign, calp2 : calp2 * swapp * latsign,
  /* minus signs give range [-180, 180). 0- converts -0 to +0. */
  azi1 : 0b0 - atan2x(-salp1, calp1) / degree,
  azi2 : 0b0 - atan2x(-salp2, calp2) / degree,
  [a12, s12, azi1, azi2, m12, M12, M21, S12]
)$

geod_inverse(g, lat1, lon1, lat2, lon2):=
geod_geninverse(g, lat1, lon1, lat2, lon2)$

SinCosSeries(sinp, sinx, cosx, c):=block([n:length(c), ar, y0, y1, n2, k],
  /* Evaluate
  * y = sinp ? sum(c[i] * sin( 2*i    * x), i, 1, n) :
  *            sum(c[i] * cos((2*i-1) * x), i, 1, n)
  * using Clenshaw summation.  where n = length(c)
  * Approx operation count = (n + 5) mult and (2 * n + 2) add */
  /* 2 * cos(2 * x) */
  ar : 2 * (cosx - sinx) * (cosx + sinx),
  /* accumulators for sum */
  if mod(n, 2)=1 then (y0 : c[n], n2 : n - 1)
  else (y0 : 0b0, n2 : n),
  y1 : 0b0,
  /* Now n2 is even */
  for k : n2 thru 1 step -2 do (
    /* Unroll loop x 2, so accumulators return to their original role */
    y1 : ar * y0 - y1 + c[k],
    y0 : ar * y1 - y0 + c[k-1]),
  if sinp then 2 * sinx * cosx * y0 /* sin(2 * x) * y0 */
  else cosx * (y0 - y1) /* cos(x) * (y0 - y1) */
  )$

/* Return [s12b, m12b, m0, M12, M21] */
Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2, cbet1, cbet2):=
block([scalep:true, C1a, C2a,
  s12b : 0b0, m12b : 0b0, m0 : 0b0, M12 : 0b0, M21 : 0b0,
  A1m1, AB1, A2m1, AB2, J12],
  /* Return m12b = (reduced length)/b; also calculate s12b = distance/b,
  and m0 = coefficient of secular term in expression for reduced length. */
  if exact then (
    m0 : - E[e_k2] * E[e_dc] / (pi/2),
    J12 : m0 *
      (sig12 + deltad(ssig2, csig2, dn2, E[e_k2], E[e_dc])
      - deltad(ssig1, csig1, dn1, E[e_k2], E[e_dc])) )
  else (C1a : C1f(eps), C2a : C2f(eps),
    A1m1 : A1m1f(eps),
    AB1 : (1 + A1m1) * (SinCosSeries(true, ssig2, csig2, C1a) -
      SinCosSeries(true, ssig1, csig1, C1a)),
    A2m1 : A2m1f(eps),
    AB2 : (1 + A2m1) * (SinCosSeries(true, ssig2, csig2, C2a) -
      SinCosSeries(true, ssig1, csig1, C2a)),
    m0 : A1m1 - A2m1,
    J12 : m0 * sig12 + (AB1 - AB2)),
  /* Missing a factor of b.
  Add parens around (csig1 * ssig2) and (ssig1 * csig2) to ensure accurate
  cancellation in the case of coincident points. */
  m12b : dn2 * (csig1 * ssig2) - dn1 * (ssig1 * csig2) - csig1 * csig2 * J12,
  /* Missing a factor of b */
  s12b : if exact then
  E[e_ec] / (pi/2) *
  (sig12 + deltae(ssig2, csig2, dn2, E[e_k2], E[e_ec])
  - deltae(ssig1, csig1, dn1, E[e_k2], E[e_ec]))
  else (1 + A1m1) * sig12 + AB1,
  if scalep then block([csig12 : csig1 * csig2 + ssig1 * ssig2,
    t : g[g_ep2] * (cbet1 - cbet2) * (cbet1 + cbet2) / (dn1 + dn2)],
    M12 : csig12 + (t * ssig2 - csig2 * J12) * ssig1 / dn1,
    M21 : csig12 - (t * ssig1 - csig1 * J12) * ssig2 / dn2 ),
  [s12b, m12b, m0, M12, M21])$

Astroid(x, y):= block(
  /* Solve k^4+2*k^3-(x^2+y^2-1)*k^2-2*y^2*k-y^2 = 0 for positive root k.
  This solution is adapted from Geocentric::Reverse. */
  [k, p : sq(x), q : sq(y), r],
  r : (p + q - 1) / 6,
  if not(q = 0b0 and r <= 0b0) then block(
    /* Avoid possible division by zero when r = 0 by multiplying equations
    for s and t by r^3 and r, resp. */
    [S : p * q / 4,            /* S = r^3 * s */
    r2 : sq(r),
    r3, disc, u, v, uv, w],
    r3 : r * r2,
    /* The discrimant of the quadratic equation for T3.  This is zero on
    the evolute curve p^(1/3)+q^(1/3) = 1 */
    disc : S * (S + 2 * r3),
    u : r,
    v, uv, w,
    if disc >= 0b0 then block([T3 : S + r3, T],
      /* Pick the sign on the sqrt to maximize abs(T3).  This minimizes loss
      of precision due to cancellation.  The result is unchanged because
      of the way the T is used in definition of u. */
      /* T3 = (r * t)^3 */
      T3 : T3 + (if T3 < 0b0 then -sqrt(disc) else sqrt(disc)),
      /* N.B. cbrtx always returns the real root.  cbrtx(-8) = -2. */
      T : cbrtx(T3),            /* T = r * t */
      /* T can be zero, but then r2 / T -> 0. */
      u : u + T + (if T # 0b0 then r2 / T else 0b0))
    else block(
      /* T is complex, but the way u is defined the result is real. */
      [ang : atan2x(sqrt(-disc), -(S + r3))],
      /* There are three possible cube roots.  We choose the root which
      avoids cancellation.  Note that disc < 0 implies that r < 0. */
      u : u + 2 * r * cos(ang / 3)),
    v : sqrt(sq(u) + q),              /* guaranteed positive */
    /* Avoid loss of accuracy when u < 0. */
    uv : if u < 0b0 then q / (v - u) else u + v, /* u+v, guaranteed positive */
    w : (uv - q) / (2 * v),           /* positive? */
    /* Rearrange expression for k to avoid loss of accuracy due to
    subtraction.  Division by 0 not possible because uv > 0, w >= 0. */
    k : uv / (sqrt(uv + sq(w)) + w))   /* guaranteed positive */
  else                /* q == 0 && r <= 0 */
  /* y = 0 with |x| <= 1.  Handle this case directly.
  for y small, positive root is k = abs(y)/sqrt(1-x^2) */
  k : 0b0,
  k)$

/* Return [sig12, salp1, calp1, salp2, calp2, dnm] */
InverseStart(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, lam12):=block(
  [ salp1 : 0b0, calp1 : 0b0, salp2 : 0b0, calp2 : 0b0, dnm : 0b0,
  /* Return a starting point for Newton's method in salp1 and calp1 (function
  value is -1).  If Newton's method doesn't need to be used, return also
  salp2 and calp2 and function value is sig12. */
  sig12 : -1b0,               /* Return value */
  /* bet12 = bet2 - bet1 in [0, pi); bet12a = bet2 + bet1 in (-pi, 0] */
  sbet12 : sbet2 * cbet1 - cbet2 * sbet1,
  cbet12 : cbet2 * cbet1 + sbet2 * sbet1,
  sbet12a : sbet2 * cbet1 + cbet2 * sbet1,
  shortline, omg12, somg12, comg12, ssig12, csig12, E:[]],
  shortline : is(cbet12 >= 0b0 and sbet12 < 0.5b0 and cbet2 * lam12 < 0.5b0),
  omg12 : lam12,
  if shortline then block([sbetm2 : sq(sbet1 + sbet2)],
    /* sin((bet1+bet2)/2)^2
     * =  (sbet1 + sbet2)^2 / ((sbet1 + sbet2)^2 + (cbet1 + cbet2)^2) */
    sbetm2 : sbetm2 / (sbetm2 + sq(cbet1 + cbet2)),
    dnm : sqrt(1 + g[g_ep2] * sbetm2),
    omg12 : omg12 / (g[g_f1] * dnm)),
  somg12 : sin(omg12), comg12 : cos(omg12),
  salp1 : cbet2 * somg12,
  calp1 : if comg12 >= 0b0
  then sbet12 + cbet2 * sbet1 * sq(somg12) / (1 + comg12)
  else sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12),
  ssig12 : hypotx(salp1, calp1),
  csig12 : sbet1 * sbet2 + cbet1 * cbet2 * comg12,
  if shortline and ssig12 < g[g_etol2] then (
    /* really short lines */
    salp2 : cbet1 * somg12,
    calp2 : sbet12 - cbet1 * sbet2 *
    (if comg12 >= 0 then sq(somg12) / (1 + comg12) else 1 - comg12),
    block([t:SinCosNorm(salp2, calp2)], salp2:t[1], calp2:t[2]),
    /* Set return value */
    sig12 : atan2x(ssig12, csig12))
  else if abs(g[g_n]) > 0.1b0 or /* No astroid calc if too eccentric */
  csig12 >= 0 or
  ssig12 >= 6 * abs(g[g_n]) * pi * sq(cbet1) then true
  /* Nothing to do, zeroth order spherical approximation is OK */
  else block(
    /* Scale lam12 and bet2 to x, y coordinate system where antipodal point
    is at origin and singular point is at y = 0, x = -1. */
    [y, lamscale, betscale,x],
    if g[g_f] >= 0 then (            /* In fact f == 0 does not get here */
      /* x = dlong, y = dlat */
      if exact then block([k2 : sq(sbet1) * g[g_ep2]],
        E : Ef(-k2, -g[g_ep2]),
        lamscale : g[g_e2]/g[g_f1] * cbet1 * 2 * E[e_hc])
      else block([k2 : sq(sbet1) * g[g_ep2],eps],
        eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
        lamscale : g[g_f] * cbet1 * A3f(g, eps) * pi),
      betscale : lamscale * cbet1,
      x : (lam12 - pi) / lamscale,
      y : sbet12a / betscale)
    else block(                    /* f < 0 */
      /* x = dlat, y = dlong */
      [cbet12a : cbet2 * cbet1 - sbet2 * sbet1,bet12a,m12b, m0],
      bet12a : atan2x(sbet12a, cbet12a),
      /* In the case of lon12 = 180, this repeats a calculation made in
      * Inverse. */
      block([r],
        r:Lengths(g, g[g_n], E, pi + bet12a,
          sbet1, -cbet1, dn1, sbet2, cbet2, dn2,
          cbet1, cbet2),
        m12b:r[2], m0:r[3]),
      x : -1 + m12b / (cbet1 * cbet2 * m0 * pi),
      betscale : if x < -0.01b0 then sbet12a / x else
      -g[g_f] * sq(cbet1) * pi,
      lamscale : betscale / cbet1,
      y : (lam12 - pi) / lamscale),
    if y > -tol1 and x > -1 - xthresh then (
      /* strip near cut */
      if g[g_f] >= 0b0 then (
        salp1 : min(1b0, -x), calp1 : - sqrt(1 - sq(salp1)))
      else (
        calp1 : max(if x > -tol1 then 0b0 else -1b0, x),
        salp1 : sqrt(1 - sq(calp1))))
    else block(
      /* Estimate alp1, by solving the astroid problem.
      Could estimate alpha1 = theta + pi/2, directly, i.e.,
        calp1 = y/k; salp1 = -x/(1+k);  for f >= 0
        calp1 = x/(1+k); salp1 = -y/k;  for f < 0 (need to check)
      However, it's better to estimate omg12 from astroid and use
      spherical formula to compute alp1.  This reduces the mean number of
      Newton iterations for astroid cases from 2.24 (min 0, max 6) to 2.12
      (min 0 max 5).  The changes in the number of iterations are as
      follows:
      change percent
         1       5
         0      78
        -1      16
        -2       0.6
        -3       0.04
        -4       0.002
      The histogram of iterations is (m = number of iterations estimating
      alp1 directly, n = number of iterations estimating via omg12, total
      number of trials = 148605):
       iter    m      n
         0   148    186
         1 13046  13845
         2 93315 102225
         3 36189  32341
         4  5396      7
         5   455      1
         6    56      0
      Because omg12 is near pi, estimate work with omg12a = pi - omg12 */
      [k : Astroid(x, y),omg12a],
      omg12a : lamscale *
      ( if g[g_f] >= 0b0 then -x * k/(1 + k) else -y * (1 + k)/k ),
      somg12 : sin(omg12a), comg12 : -cos(omg12a),
      /* Update spherical estimate of alp1 using omg12 instead of lam12 */
      salp1 : cbet2 * somg12,
      calp1 : sbet12a - cbet2 * sbet1 * sq(somg12) / (1 - comg12))),
  if salp1 > 0 then              /* Sanity check on starting guess */
  block([t:SinCosNorm(salp1, calp1)], salp1:t[1], calp1:t[2])
  else ( salp1 : 1b0, calp1 : 0b0 ),
  [sig12, salp1, calp1, salp2, calp2, dnm]
  )$

/* Return [lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
(E or eps), (omg12 or domg12), dlam12]
*/
Lambda12(g, sbet1, cbet1, dn1, sbet2, cbet2, dn2, salp1, calp1, diffp):=
block([salp2 : 0b0, calp2 : 0b0, sig12 : 0b0,
  ssig1 : 0b0, csig1 : 0b0, ssig2 : 0b0, csig2 : 0b0, eps : 0b0, E : [],
  domg12 : 0b0, dlam12 : 0b0,
  salp0, calp0,
  somg1, comg1, somg2, comg2, cchi1, cchi2, omg12:0, lam12,
  B312, h0, k2],
  if sbet1 = 0b0 and calp1 = 0b0 then
  /* Break degeneracy of equatorial line.  This case has already been
  handled. */
  calp1 : -tiny,
  /* sin(alp1) * cos(bet1) = sin(alp0) */
  salp0 : salp1 * cbet1,
  calp0 : hypotx(calp1, salp1 * sbet1), /* calp0 > 0 */
  /* tan(bet1) = tan(sig1) * cos(alp1)
  tan(omg1) = sin(alp0) * tan(sig1) = tan(omg1)=tan(alp1)*sin(bet1) */
  ssig1 : sbet1, somg1 : salp0 * sbet1,
  csig1 : comg1 : calp1 * cbet1,
  /* Without normalization we have schi1 = somg1. */
  if exact then cchi1 : g[g_f1] * dn1 * comg1,
  block([t:SinCosNorm(ssig1, csig1)], ssig1:t[1], csig1:t[2]),
  /* SinCosNorm (&somg1, &comg1); -- don't need to normalize! */
  /* Enforce symmetries in the case abs(bet2) = -bet1.  Need to be careful
  about this case, since this can yield singularities in the Newton
  iteration.
  sin(alp2) * cos(bet2) = sin(alp0) */
  salp2 : if cbet2 # cbet1 then salp0 / cbet2 else salp1,
  /* calp2 = sqrt(1 - sq(salp2))
         = sqrt(sq(calp0) - sq(sbet2)) / cbet2
  and subst for calp0 and rearrange to give; choose positive sqrt
  to give alp2 in [0, pi/2]. */
  calp2 : if cbet2 # cbet1 or abs(sbet2) # -sbet1 then
  sqrt(sq(calp1 * cbet1) +
    (if cbet1 < -sbet1 then
      (cbet2 - cbet1) * (cbet1 + cbet2) else
      (sbet1 - sbet2) * (sbet1 + sbet2))) / cbet2 else
  abs(calp1),
  /* tan(bet2) = tan(sig2) * cos(alp2)
  tan(omg2) = sin(alp0) * tan(sig2). */
  ssig2 : sbet2, somg2 : salp0 * sbet2,
  csig2 : comg2 : calp2 * cbet2,
  /* Without normalization we have schi2 = somg2. */
  if exact then cchi2 : g[g_f1] * dn2 * comg2,
  block([t:SinCosNorm(ssig2, csig2)], ssig2:t[1], csig2:t[2]),
  /* SinCosNorm (&somg2, &comg2); -- don't need to normalize! */
  /* sig12 = sig2 - sig1, limit to [0, pi] */
  sig12 : atan2x(max(csig1 * ssig2 - ssig1 * csig2, 0b0),
    csig1 * csig2 + ssig1 * ssig2),
  /* omg12 = omg2 - omg1, limit to [0, pi] */
  omg12 : atan2x(max(comg1 * somg2 - somg1 * comg2, 0b0),
    comg1 * comg2 + somg1 * somg2),
  k2 : sq(calp0) * g[g_ep2],
  if exact then block([chi12],
    E : Ef(-k2, -g[g_ep2]),
    chi12 : atan2x(max(cchi1 * somg2 - somg1 * cchi2, 0b0),
                       cchi1 * cchi2 + somg1 * somg2),
    lam12 : chi12 -
      g[g_e2]/g[g_f1] * salp0 * E[e_hc] / (pi/2) *
      (sig12 + deltah(ssig2, csig2, dn2, E[e_k2], E[e_alpha2], E[e_hc]) -
       deltah(ssig1, csig1, dn1, E[e_k2], E[e_alpha2], E[e_hc]) ) )
  else (eps : k2 / (2 * (1 + sqrt(1 + k2)) + k2),
    C3a : C3f(g, eps),
    B312 : (SinCosSeries(true, ssig2, csig2, C3a) -
      SinCosSeries(true, ssig1, csig1, C3a)),
    h0 : -g[g_f] * A3f(g, eps),
    domg12 : salp0 * h0 * (sig12 + B312),
    lam12 : omg12 + domg12),
  if diffp then (
    if calp2 = 0b0 then
    dlam12 : - 2 * g[g_f1] * dn1 / sbet1
    else (block([r],
        r:Lengths(g, eps, E, sig12, ssig1, csig1, dn1, ssig2, csig2, dn2,
          cbet1, cbet2),
        dlam12:r[2]),
      dlam12 : dlam12 * g[g_f1] / (calp2 * cbet2))),
  [lam12, salp2, calp2, sig12, ssig1, csig1, ssig2, csig2,
  if exact then E else eps, if exact then omg12 else domg12, dlam12])$

A3f(g, eps):=block(
  /* Evaluate sum(A3x[k] * eps^k, k, 0, nA3x-1) by Horner's method */
  [v : 0b0],
  for i:nA3x step -1 thru 1 do
  v : eps * v + g[g_A3x][i],
  v)$

C3f(g, eps):=block(
  /* Evaluate C3 coeffs by Horner's method
  * Elements c[1] thru c[nC3 - 1] are set */
  [i, j, k, mult : 1b0, c : makelist(0, i, 1, nC3-1)],
  j : nC3x,
  for k : nC3-1 step -1 thru 1 do block(
    [t : 0b0],
    for i : nC3 - k step -1 thru 1 do (
      t : eps * t + g[g_C3x][j],
      j : j - 1),
    c[k] : t),
  for k:1 thru nC3-1 do (
    mult : mult * eps,
    c[k] : c[k] * mult),
  c)$

C4f(g, eps):=block(
  /* Evaluate C4 coeffs by Horner's method
  * Elements c[1] thru c[nC4] are set */
  [i, j, k, mult : 1b0, c : makelist(0, i, 1, nC4)],
  j : nC4x,
  for k : nC4-1 step -1 thru 0 do block(
    [t : 0b0],
    for i : nC4 - k step -1 thru 1 do (
      t : eps * t + g[g_C4x][j],
      j : j - 1),
    c[k+1] : t),
  for k : 2 thru nC4 do (
    mult : mult * eps,
    c[k] : c[k] * mult),
  c)$

transit(lon1, lon2):=block([lon12],
  /* Return 1 or -1 if crossing prime meridian in east or west direction.
  * Otherwise return zero. */
  /* Compute lon12 the same way as Geodesic::Inverse. */
  lon1 : AngNormalize(lon1),
  lon2 : AngNormalize(lon2),
  lon12 : AngDiff(lon1, lon2),
  if lon1 < 0b0 and lon2 >= 0b0 and lon12 > 0b0 then 1 else
  (if lon2 < 0b0 and lon1 >= 0b0 and lon12 < 0b0 then -1 else 0))$

/* Return [P, A, mins, maxs] */
geod_polygonarea(g, points) := block([n:length(points), crossings : 0,
  area0 : 4 * pi * g[g_c2], A : 0, P : 0, mins : g[g_a] * 100, maxs : 0b0],
  for i : 1 thru n do block(
    [ s12, S12, r ],
    r:geod_geninverse(g,
      points[i][1], points[i][2],
      points[mod(i,n)+1][1], points[mod(i,n)+1][2]),
    s12:r[2], S12:r[8],
    if s12 > maxs then maxs:s12,
    if s12 < mins then mins:s12,
    P : P + s12,
    /* The minus sign is due to the counter-clockwise convention */
    A : A - S12,
    crossings : crossings + transit(points[i][2], points[mod(i,n)+1][2])),
  if mod(crossings, 2) = 1 then
  A : A + (if A < 0b0 then 1 else -1) * area0/2,
  /* Put area in (-area0/2, area0/2] */
  if A > area0/2 then
  A : A - area0
  else if A <= -area0/2 then
  A : A + area0,
  [P, A, mins, maxs])$

wgs84:geod_init(6378137b0, 1/298.257223563b0)$

flat(a, GM, omega, J2):=block(
  [e2:3*J2, K : 2 * (a*omega)^2 * a / (15 * GM), e2a, q0],
  for j:0 thru 100 do (
    e2a:e2,q0:qf(e2/(1-e2)),
    e2:3*J2+K*e2*sqrt(e2)/q0,
    if e2 = e2a then return(done)),
  e2/(1+sqrt(1-e2)))$
qf(ep2):=block([ep,fpprec:3*fpprec],ep:sqrt(ep2),
  ((1 + 3/ep2) * atan(ep) - 3/ep)/2)$
/* 1/298.257222100882711243162836607614495 */
fgrs80:flat(6378137b0, 3986005b8, 7292115b-11, 108263b-8)$
grs80:geod_init(6378137b0, fgrs80)$