/*

Arbitrary precision Transverse Mercator Projection

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

Reference:

   Charles F. F. Karney,
   Transverse Mercator with an accuracy of a few nanometers,
   J. Geodesy 85(8), 475-485 (Aug. 2011).
   DOI 10.1007/s00190-011-0445-3
   preprint http://arxiv.org/abs/1002.1417
   resource page http://geographiclib.sf.net/tm.html

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),
  n:f/(2-f),
  '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

*/

/* sqrt(x^2 + y^2) -- Real only */
hypot(x,y):=sqrt(x^2 + y^2)$

/* log(1 + x) -- Real only */
log1p(x) := block([y : 1b0+x],
  if y = 1b0 then x else x*log(y)/(y - 1))$

/* Real only */
/* Some versions of Maxima have a buggy atanh
atnh(x) := block([y : abs(x)],
  y : log1p(2 * y/(1 - y))/2,
  if x < 0 then -y else y)$ */
atnh(x) := atanh(x)$

/* exp(x)-1 -- Real only */
expm1(x) := block([y : exp(bfloat(x)),z],
  z : y - 1b0,
  if abs(x) > 1b0 then z else if z = 0b0 then x else x * z/log(y))$

/* Real only */
/* Some versions of Maxima have a buggy sinh */
snh(x) := block([u : expm1(x)],
  (u / (u + 1)) * (u + 2) /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(snh(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(snh(psia),cos(lam))
      +%i*asinh(sin(lam)/sqrt(cos(lam)^2 + snh(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+snh(psir)^2))*(1+e2/2),
  vv:atan2(cos(lam), snh(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 forward */

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])$

/* Lee/Thompson's method reverse */

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])$

/* Return lat/lon/x/y for a point specified in Thompson coords */
/* Pick u in [0, kcu] and v in [0, kcv] */
lltm(u,v):=block([jacr,jaci,psi,lam,phi,c,gam,scale,sigma,x,y],
  u:bfloat(u), v:bfloat(v),
  jacr:sncndn(u,1-e2),
  jaci:sncndn(v,e2),
  psi:psi_thom(u+%i*v),
  sigma:sigma_thom(u+%i*v),
  x:imagpart(sigma)*k0*a,y:realpart(sigma)*k0*a,
  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,x,y,gam/degree,k0*scale])$

/* Gauss-Krueger series to order n^i forward

Uses the array functions

 a1_a[i](n), zeta_a[i](z,n), zeta_d[i](z,n), zetap_a[i](s,n), zetap_d[i](s,n),

defined in tmseries.mac.
*/

tms(phi,lam,i):=block([psi,xip,etap,z,sigma,sp,gam,k,b1],
  phi:phi*degree,
  lam:lam*degree,
  psi:psi_phi(phi),
  xip:atan2(snh(psi), cos(lam)),
  etap:asinh(sin(lam)/hypot(snh(psi),cos(lam))),
  k:sqrt(1 - e2*sin(phi)^2)/(cos(phi)*hypot(snh(psi),cos(lam))),
  gam:atan(tan(xip)*tanh(etap)),
  z:xip+%i*etap,
  b1:a1_a[i](n),
  sigma:rectform(b1*zeta_a[i](z,n)),
  sp:rectform(zeta_d[i](z,n)),
  gam : gam - atan2(imagpart(sp),realpart(sp)),
  k : k * b1 * cabs(sp),
  [imagpart(sigma)*k0*a,realpart(sigma)*k0*a,gam/degree,k*k0])$

/* Gauss-Krueger series to order n^i reverse */

lls(x,y,i):=block([sigma,b1,s,z,zp,xip,etap,s,c,r,gam,k,lam,psi,phi],
  sigma:y/(a*k0)+%i*x/(a*k0),
  b1:a1_a[i](n),
  s:rectform(sigma/b1),
  z:rectform(zetap_a[i](s,n)),
  zp:rectform(zetap_d[i](s,n)),
  gam : atan2(imagpart(zp), realpart(zp)),
  k : b1 / cabs(zp),
  xip:realpart(z),
  etap:imagpart(z),
  s:snh(etap),
  c:cos(xip),
  r:hypot(s, c),
  lam:atan2(s, c),
  psi : asinh(sin(xip)/r),
  phi :phi_psi(psi),
  k : k * sqrt(1 - e2*sin(phi)^2) * r/cos(phi),
  gam : gam + atan(tan(xip) * tanh(etap)),
  [phi/degree,lam/degree,gam/degree,k*k0])$

/* Approx geodesic distance valid for small displacements */

dist(phi0,lam0,phi,lam):=block([dphi,dlam,nn,hlon,hlat],
  dphi:(phi-phi0)*degree,
  dlam:(lam-lam0)*degree,
  phi0:phi0*degree,
  lam0:lam0*degree,
  nn : 1/sqrt(1 - e2 * sin(phi0)^2),
  hlon : cos(phi0) * nn,
  hlat : (1 - e2) * nn^3,
  a * hypot(dphi*hlat, dlam*hlon))$

/* Compute truncation errors for all truncation levels */

check(phi,lam):=block([vv,x,y,gam,k,vf,vb,errf,errr,err2,errlist],
  phi:min(90-0.01b0,phi),
  lam:min(90-0.01b0,lam),
  vv:tm(phi,lam),
  errlist:[],
  x:vv[1], y:vv[2], gam:vv[3], k:vv[4],
  for i:1 thru maxpow do (
    vf:tms(phi,lam,i),
    errf:hypot(vf[1]-x,vf[2]-y)/k,
    errfg:abs(vf[3]-gam),
    errfk:abs((vf[4]-k)/k),
    vb:lls(x,y,i),
    errr:dist(phi,lam,vb[1],vb[2]),
    errrg:abs(vb[3]-gam),
    errrk:abs((vb[4]-k)/k),
    errlist:append(errlist,
      [max(errf, errr), max(errfg, errrg), max(errfk, errrk)])),
  errlist)$

/* Max of output of check over a set of points */

checka(lst):=block([errlist:[],errx],
  for i:1 thru 3*maxpow do errlist:cons(0b0,errlist),
  for vv in lst do (
    errx:check(vv[1],vv[2]),
    for i:1 thru 3*maxpow do errlist[i]:max(errlist[i],errx[i])),
  errlist)$