/*

Arbitrary precision Transverse Mercator Projection

Written by Charles Karney <charles@karney.com>
http://charles.karney.info/geographic

$Id: tm.mac 6500 2009-01-11 21:22:41Z ckarney $

The parameters for the transformation are set by

setparams(a,invf,k0)$
    sets the major radius, inverse flattening, and central scale
    factor. The default is
      setparams(6378137b0, 298.257223563b0, 0.9996b0)$
    appropriate for UTM applications.

tm(lat,lon);
    takes lat and lon args (in degrees) and returns
      [x, y, convergence, scale]
    [x, y] do not include false eastings/northings but do include the
    scale factor k0.  convergence is in degrees.

ll(x,y);
    takes x and y args (in meters) and returns
      [lat, lon, convergence, scale].

Example:

$ maxima
Maxima 5.15.0 http://maxima.sourceforge.net
Using Lisp CLISP 2.43 (2007-11-18)
Distributed under the GNU Public License. See the file COPYING.
Dedicated to the memory of William Schelter.
The function bug_report() provides bug reporting information.
(%i1) load("tm.mac")$
(%i2) tm(10b0,20b0);
(%o2) [2.235209504622466691587930831718465965864199221939781808953597771095103\
6690000464b6, 1.17529734503138466792126931904154130080533935727351398258511134\
68541970512119385b6, 3.6194756227592979778565787394402350354250845160819430786\
093514889500602612857052b0, 1.062074627142564335518604915718789933200854739344\
8664109599248189291146283796933b0]
(%i3) ll(%[1],%[2]);
(%o3) [1.0b1, 2.0b1, 3.6194756227592979778565787394402350354250845160819430786\
093514889500602612857053b0, 1.062074627142564335518604915718789933200854739344\
8664109599248189291146283796933b0]
(%i4) float(%o2);
(%o4) [2235209.504622467, 1175297.345031385, 3.619475622759298,
                                                             1.062074627142564]
(%i5) float(%o3);
(%o5)         [10.0, 20.0, 3.619475622759298, 1.062074627142564]

This implements GeographicLib::TransverseMercatorExact (i.e., Lee, 1976)
using bfloats.  However fewer changes from Lee 1976 have been made since
we rely more heavily on the high precision to deal with problem cases.

To change the precision, change fpprec below and reload.

*/

fpprec:80$
load("ellint.mac")$ /* Load elliptic functions */

tol:0.1b0^fpprec$
tol1:0.1b0*sqrt(tol)$ /* For Newton's method */
tol2:sqrt(0.01*tol*tol1)$ /* Also for Newton's method but more conservative */
ahypover:log(10b0^fpprec)+2$

pi:bfloat(%pi)$
degree:pi/180$
ratprint:false$
debugprint:false$
setparams(a1,invf,k1):=(a:bfloat(a1),f:1/bfloat(invf),k0:bfloat(k1),
  e2:f*(2-f),
  e:sqrt(e2),
  kcu:kc(e2),
  kcv:kc(1-e2),
  ecu:ec(e2),
  ecv:ec(1-e2),
  'done)$
setparams(6378137b0, 298.257223563b0, 0.9996b0)$  /* WGS 84 */
/* setparams(6378388b0, 297b0, 0.9996b0)$ International */
/* setparams(1/ec(0.01b0), 30*sqrt(11b0)+100, 1b0)$ testing, eps = 0.1*/

/*
Interpret x_y(y) as x <- y, i.e., "transform quantity y to quantity x"

Let

phi = geodetic latitude
psi = isometric latitude ( + i * lambda )
sigma = TM coords
thom = Thompson coords

*/

/* Real only */
atnh(x):=log((1+x)/(1-x))/2$

/* Real only */
psi_phi(phi):=block([s:sin(phi)],
  asinh(s/max(cos(phi),0.1b0*tol)) - e * atnh(e * s))$

/* Real only */
phi_psi(psi):=block([q:psi,t,dq],
  for i do (
    t:tanh(q),
    dq : -(q - e * atnh(e * t) - psi) * (1 - e2 * t^2) / (1 - e2),
    q : q + dq,
    if debugprint then print(float(q), float(dq)),
    if abs(dq) < tol1 then return(false)),
  atan(sinh(q)))$

psi_thom_comb(w):=block([jacr:sncndn(bfloat(realpart(w)),1-e2),
  jaci:sncndn(bfloat(imagpart(w)),e2),d,d1,d2],
  d:(1-e2)*(jaci[2]^2 + e2 * (jacr[1] * jaci[1])^2)^2,
  d1:sqrt(jacr[2]^2 + (1-e2) * (jacr[1] * jaci[1])^2),
  d2:sqrt(e2 * jacr[2]^2 + (1-e2) * jaci[2]^2),
[
  (if d1 > 0b0 then asinh(jacr[1]*jaci[3]/ d1) else signnum(snu) * ahypover)
  - (if d2 > 0b0 then e * asinh(e * jacr[1] / d2) else signnum(snu) * ahypover)
  + %i * (if d1 > 0b0 and d2 > 0b0 then
    atan2(jacr[3]*jaci[1],jacr[2]*jaci[2])
    - e * atan2(e*jacr[2]*jaci[1],jacr[3]*jaci[2]) else 0),
  jacr[2]*jacr[3]*jaci[3]*(jaci[2]^2-e2*(jacr[1]*jaci[1])^2)/d
  -%i * jacr[1]*jaci[1]*jaci[2]*((jacr[3]*jaci[3])^2+e2*jacr[2]^2)/d]
)$

psi_thom(w):=block([tt:psi_thom_comb(w)],tt[1])$
inv_diff_psi_thom(w):=block([tt:psi_thom_comb(w)],tt[2])$

w0a(psi):=block([lam:bfloat(imagpart(psi)),psia:bfloat(realpart(psi))],
  rectform(kcu/(pi/2)*( atan2(sinh(psia),cos(lam))
      +%i*asinh(sin(lam)/sqrt(cos(lam)^2 + sinh(psia)^2)))))$

w0c(psi):=block([m,a,dlam],
  dlam:bfloat(imagpart(psi))-pi/2*(1-e),
  psi:bfloat(realpart(psi)),
  m:sqrt(psi^2+dlam^2)*3/(1-e2)/e,
  a:if m = 0b0 then 0 else atan2(dlam-psi, psi+dlam) - 0.75b0*pi,
  m:m^(1/3), a:a/3,
  m*cos(a)+%i*(m*sin(a)+kcv))$

w0d(psi):=block([psir:-realpart(psi)/e+1b0,lam:(pi/2-imagpart(psi))/e,uu,vv],
  uu:asinh(sin(lam)/sqrt(cos(lam)^2+sinh(psir)^2))*(1+e2/2),
  vv:atan2(cos(lam), sinh(psir)) *(1+e2/2),
  (-uu+kcu) + %i * (-vv+kcv))$

w0m(psi):=if realpart(psi)<-e/2*pi/2 and
imagpart(psi)>pi/2*(1-2*e) and
realpart(psi) < imagpart(psi)-(pi/2*(1-e)) then w0d(psi) else
if realpart(psi)<e*pi/2 and imagpart(psi)>pi/2*(1-2*e)
then w0c(psi) else w0a(psi)$
w0(psi):=w0m(psi)$

thom_psi(psi):=block([w:w0(psi),dw,v,vv],
if not(abs(psi-pi/2*(1-e)*%i) < e * tol^0.6b0) then
  for i do (
    if i > 100 then error("too many iterations"),
    vv:psi_thom_comb(w),
    v:vv[1],
    dw:-rectform((v-psi)*vv[2]),
    w:w+dw,
    dw:abs(dw),
    if debugprint then print(float(w),float(dw)),
    /* error is approx dw^2/2 */
    if dw < tol2 then return(false)
    ),
  w
  )$

sigma_thom_comb(w):=block([u:bfloat(realpart(w)),v:bfloat(imagpart(w)),
  jacr,jaci,phi,iu,iv,den,den1,er,ei,dnr,dni],
  jacr:sncndn(u,1-e2),jaci:sncndn(v,e2),
  er:eirx(jacr[1],jacr[2],jacr[3],e2,ecu),
  ei:eirx(jaci[1],jaci[2],jaci[3],1-e2,ecv),
  den:e2*jacr[2]^2+(1-e2)*jaci[2]^2,
  den1:(1-e2)*(jaci[2]^2 + e2 * (jacr[1] * jaci[1])^2)^2,
  dnr:jacr[3]*jaci[2]*jaci[3],
  dni:-e2*jacr[1]*jacr[2]*jaci[1],
[  er - e2*jacr[1]*jacr[2]*jacr[3]/den
  + %i*(v - ei + (1-e2)*jaci[1]*jaci[2]*jaci[3]/den),
  (dnr^2-dni^2)/den1 + %i * 2*dnr*dni/den1])$

sigma_thom(w):=block([tt:sigma_thom_comb(w)],tt[1])$
inv_diff_sigma_thom(w):=block([tt:sigma_thom_comb(w)],tt[2])$

wx0a(sigma):=rectform(sigma*kcu/ecu)$
wx0b(sigma):=block([m,aa],
  sigma:rectform(sigma-%i*(kcv-ecv)),
  m:abs(sigma)*3/(1-e2),
  aa:atan2(imagpart(sigma),realpart(sigma)),
  if aa<-pi/2 then aa:aa+2*pi,
  aa:aa-pi,
  rectform(m^(1/3)*(cos(aa/3b0)+%i*sin(aa/3b0))+%i*kcv))$
wx0c(sigma):=rectform(1/(sigma-(ecu+%i*(kcv-ecv))) + kcu+%i*kcv)$
wx0m(sigma):=block([eta:bfloat(imagpart(sigma)),
  xi:bfloat(realpart(sigma))],
  if eta > 1.25b0 * (kcv-ecv) or (xi < -0.25*ecu and xi < eta-(kcv-ecv)) then
  wx0c(sigma) else
  if (eta > 0.75b0 * (kcv-ecv) and xi < 0.25b0 * ecu) or
  eta > kcv-ecv or xi < 0 then wx0b(sigma) else wx0a(sigma))$
wx0(sigma):=wx0m(sigma)$
thom_sigma(sigma):=block([w:wx0(sigma),dw,v,vv],
  for i do (
    if i > 100 then error("too many iterations"),
    vv:sigma_thom_comb(w),
    v:vv[1],
    dw:-rectform((v-sigma)*vv[2]),
    w:w+dw,
    dw:abs(dw),
    if debugprint then print(float(w),float(dw)),
    /* error is approx dw^2/2 */
    if dw < tol2 then return(false)
    ),
  w
  )$

/* Lee/Thompson's method */

tm(phi,lam):=block([psi,thom,jacr,jaci,sigma,gam,scale,c],
  phi:phi*degree,
  lam:lam*degree,
  psi:psi_phi(phi),
  thom:thom_psi(psi+%i*lam),
  jacr:sncndn(bfloat(realpart(thom)),1-e2),
  jaci:sncndn(bfloat(imagpart(thom)),e2),
  sigma:sigma_thom(thom),
  c:cos(phi),
  if c > tol1 then (
    gam:atan2((1-e2)*jacr[1]*jaci[1]*jaci[2],
      jacr[2]*jacr[3]*jaci[3]),
    scale:sqrt(1-e2 + e2 * c^2)/c*
    sqrt(((1-e2)*jaci[1]^2 + (jacr[2]*jaci[3])^2)/
      (e2*jacr[2]^2 + (1-e2)*jaci[2]^2)))
  else (gam : lam, scale : 1b0),
  [imagpart(sigma)*k0*a,realpart(sigma)*k0*a,gam/degree,k0*scale])$

ll(x,y):=block([sigma,thom,jacr,jaci,psi,lam,phi,gam,scale,c],
  sigma:y/(a*k0)+%i*x/(a*k0),
  thom:thom_sigma(sigma),
  jacr:sncndn(bfloat(realpart(thom)),1-e2),
  jaci:sncndn(bfloat(imagpart(thom)),e2),
  psi:psi_thom(thom),
  lam:bfloat(imagpart(psi)),
  psi:bfloat(realpart(psi)),
  phi:phi_psi(psi),
  c:cos(phi),
  if c > tol1 then (
    gam:atan2((1-e2)*jacr[1]*jaci[1]*jaci[2],
      jacr[2]*jacr[3]*jaci[3]),
    scale:sqrt(1-e2 + e2 * c^2)/c*
    sqrt(((1-e2)*jaci[1]^2 + (jacr[2]*jaci[3])^2)/
      (e2*jacr[2]^2 + (1-e2)*jaci[2]^2)))
  else (gam : lam, scale : 1b0),
  [phi/degree,lam/degree,gam/degree,k0*scale])$