/********************************************************************
** @source JEEPS arithmetic/conversion functions
**
** @author Copyright (C) 1999 Alan Bleasby
** @version 1.0
** @modified Dec 28 1999 Alan Bleasby. First version
** @modified Copyright (C) 2024 Robert Lipe
** @@
**
** This library is free software; you can redistribute it and/or
** modify it under the terms of the GNU Library General Public
** License as published by the Free Software Foundation; either
** version 2 of the License, or (at your option) any later version.
**
** This library is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
** Library General Public License for more details.
**
** You should have received a copy of the GNU Library General Public
** License along with this library; if not, write to the
** Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
** Boston, MA  02110-1301, USA.
********************************************************************/
#include "jeeps/gpsmath.h"

#include <cmath>             // for sin, tan, cos, pow, log, sqrt, asin, atan, exp, fabs, round
#include <cstdint>           // for int32_t
#include <cstdlib>           // for abs
#include <cstring>           // for strcmp, strcpy
#include <ctime>             // for time_t

#include <QString>           // for QString

#include "defs.h"            // for case_ignore_strcmp, gbFatal, CSTR
#include "jeeps/gpsdatum.h"  // for GPS_ODatum, GPS_OEllipse, GPS_Datums, GPS_Ellipses, UKNG, GPS_SDatum_Alias, GPS_SDatum, GPS_DatumAliases, GPS_PDatum, GPS_PDatum_Alias

static constexpr bool use_exact_helmert_inverse = false;

static int32_t GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32_t* zone,
                                            char* zc, double* Mc, double* E0,
                                            double* N0, double* F0);
static int32_t GPS_Math_UTM_Param_To_Mc(int32_t zone, char zc, double* Mc,
                                        double* E0, double* N0, double* F0);



/* @func GPS_Math_Deg_To_Rad *******************************************
**
** Convert degrees to radians
**
** @param [r] v [double] degrees
**
** @return [double] radians
************************************************************************/

double GPS_Math_Deg_To_Rad(double v)
{
  return v*(GPS_PI/180.);
}



/* @func GPS_Math_Rad_To_Deg *******************************************
**
** Convert radians to degrees
**
** @param [r] v [double] radians
**
** @return [double] degrees
************************************************************************/

double GPS_Math_Rad_To_Deg(double v)
{
  return v*(180./GPS_PI);
}



/* @func GPS_Math_Deg_To_DegMin *****************************************
**
** Convert degrees to degrees and minutes
**
** @param [r] v [double] degrees
** @param [w] d [int32 *]  whole degrees
** @param [w] m [double *] minutes
**
** @return [void]
************************************************************************/

void GPS_Math_Deg_To_DegMin(double v, int32_t* d, double* m)
{
	int32_t sign;

  if (v<0.) {
    v *= -1.;
    sign = 1;
  } else {
    sign = 0;
  }

  *d = (int32_t)v;
  *m = (v-(double)*d) * 60.0;
  if (*m>59.999) {
    ++*d;
    *m = 0.0;
  }

  if (sign) {
    *d = -*d;
  }

  return;
}



/* @func GPS_Math_DegMin_To_Deg *****************************************
**
** Convert degrees and minutes to degrees
**
** @param [r] d [int32] whole degrees
** @param [r] m [double] minutes
** @param [w] deg [double *] degrees
**
** @return [void]
************************************************************************/

void GPS_Math_DegMin_To_Deg(int32_t d, double m, double* deg)
{

  *deg = ((double)abs(d)) + m / 60.0;
  if (d<0) {
    *deg = -*deg;
  }

  return;
}



/* @func GPS_Math_Deg_To_DegMinSec *************************************
**
** Convert degrees to degrees, minutes and seconds
**
** @param [r] v [double] degrees
** @param [w] d [int32 *]  whole degrees
** @param [w] m [int32 *]  whole minutes
** @param [w] s [double *] seconds
**
** @return [void]
************************************************************************/

void GPS_Math_Deg_To_DegMinSec(double v, int32_t* d, int32_t* m, double* s)
{
	int32_t sign;
  double t;

  if (v<0.) {
    v *= -1.;
    sign = 1;
  } else {
    sign = 0;
  }

  *d = (int32_t)v;
  t  = (v -(double)*d) * 60.0;
  *m = (v-(double)*d) * 60.0;
  *s = (t - (int32_t)t) * 60.0;

  if (*s>59.999) {
    ++t;
    *s = 0.0;
  }


  if (t>59.999) {
    ++*d;
    t = 0;
  }

  *m = (int32_t)t;

  if (sign) {
    *d = -*d;
  }

  return;
}



/* @func GPS_Math_DegMinSec_To_Deg *****************************************
**
** Convert degrees, minutes and seconds to degrees
**
** @param [r] d [int32] whole degrees
** @param [r] m [int32] whole minutes
** @param [r] s [double] seconds
** @param [w] deg [double *] degrees
**
** @return [void]
************************************************************************/

void GPS_Math_DegMinSec_To_Deg(int32_t d, int32_t m, double s, double* deg)
{

  *deg = ((double)abs(d)) + ((double)m + s / 60.0) / 60.0;
  if (d<0) {
    *deg = -*deg;
  }

  return;
}



/* @func GPS_Math_Metres_To_Feet *******************************************
**
** Convert metres to feet
**
** @param [r] v [double] metres
**
** @return [double] feet
************************************************************************/

double GPS_Math_Metres_To_Feet(double v)
{
  return v/0.3048;
}



/* @func GPS_Math_Feet_To_Metres *******************************************
**
** Convert feet to metres
**
** @param [r] v [double] feet
**
** @return [double] metres
************************************************************************/

double GPS_Math_Feet_To_Metres(double v)
{
  return v * 0.3048;
}



/* @func GPS_Math_Deg_To_Semi *******************************************
**
** Convert degrees to semi-circles
**
** @param [r] v [double] degrees
**
** @return [int32] semicircles
************************************************************************/

int32_t GPS_Math_Deg_To_Semi(double v)
{
  return round(((double)(1U<<31) / 180.0) * v);
}



/* @func GPS_Math_Semi_To_Deg *******************************************
**
** Convert semi-circles to degrees
**
** @param [r] v [int32] semi-circles
**
** @return [double] degrees
************************************************************************/

double GPS_Math_Semi_To_Deg(int32_t v)
{
  return (((double)v / (double)(1U<<31)) * 180.0);
}



/* @func GPS_Math_Utime_To_Gtime *******************************************
**
** Convert Unix time (1970) to GPS time (1990)
**
** @param [r] v [time_t] Unix time
**
** @return [time_t] GPS time
************************************************************************/

time_t GPS_Math_Utime_To_Gtime(time_t v)
{
  return v-631065600;
}



/* @func GPS_Math_Gtime_To_Utime *******************************************
**
** Convert GPS time (1990) to Unix time (1970)
**
** @param [r] v [time_t] GPS time
**
** @return [time_t] Unix time
************************************************************************/

time_t GPS_Math_Gtime_To_Utime(time_t v)
{
  return v+631065600;
}




/* @func GPS_Math_LatLonH_To_XYZ **********************************
**
** Convert latitude and longitude and ellipsoidal height to
** X, Y & Z coordinates
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] H [double] ellipsoidal height (metres)
** @param [w] x [double *] X
** @param [w] y [double *] Y
** @param [w] z [double *] Z
** @param [r] a [double] semi-major axis (metres)
** @param [r] b [double] semi-minor axis (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_LatLonH_To_XYZ(double phi, double lambda, double H,
                             double* x, double* y, double* z,
                             double a, double b)
{
  double esq;
  double nu;

  phi    = GPS_Math_Deg_To_Rad(phi);
  lambda = GPS_Math_Deg_To_Rad(lambda);


  esq   = ((a*a)-(b*b)) / (a*a);

  nu    = a / pow((1.0-esq*sin(phi)*sin(phi)),0.5);
  *x    = (nu+H) * cos(phi) * cos(lambda);
  *y    = (nu+H) * cos(phi) * sin(lambda);
  *z    = ((1.0-esq)*nu+H) * sin(phi);

  return;
}




/* @func GPS_Math_XYX_To_LatLonH ***************************************
**
** Convert XYZ coordinates to latitude and longitude and ellipsoidal height
**
** @param [w] phi [double] latitude (deg)
** @param [w] lambda [double] longitude (deg)
** @param [w] H [double] ellipsoidal height (metres)
** @param [r] x [double *] X
** @param [r] y [double *] Y
** @param [r] z [double *] Z
** @param [r] a [double] semi-major axis (metres)
** @param [r] b [double] semi-minor axis (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_XYZ_To_LatLonH(double* phi, double* lambda, double* H,
                             double x, double y, double z,
                             double a, double b)
{
  double esq;
  double nu=0.0;
  double phix;
  double nphi;
  double rho;
  int32_t t1 = 0;
  int32_t t2 = 0;

  if (x<0.0 && y>=0.0) {
    t1=1;
  }
  if (x<0.0 && y<0.0) {
    t2=1;
  }

  rho  = pow(((x*x)+(y*y)),0.5);
  esq  = ((a*a)-(b*b)) / (a*a);
  phix = atan(z/((1.0 - esq) * rho));
  nphi = 1e20;

  while (fabs(phix-nphi)>0.00000000001) {
    nphi  = phix;
    nu    = a / pow((1.0-esq*sin(nphi)*sin(nphi)),0.5);
    phix  = atan((z+esq*nu*sin(nphi))/rho);
  }

  *phi    = GPS_Math_Rad_To_Deg(phix);
  *H      = (rho / cos(phix)) - nu;
  *lambda = GPS_Math_Rad_To_Deg(atan(y/x));

  if (t1) {
    *lambda += 180.0;
  }
  if (t2) {
    *lambda -= 180.0;
  }

  return;
}



/* @func GPS_Math_Airy1830LatLonH_To_XYZ **********************************
**
** Convert Airy 1830 latitude and longitude and ellipsoidal height to
** X, Y & Z coordinates
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] H [double] ellipsoidal height (metres)
** @param [w] x [double *] X
** @param [w] y [double *] Y
** @param [w] z [double *] Z
**
** @return [void]
************************************************************************/
void GPS_Math_Airy1830LatLonH_To_XYZ(double phi, double lambda, double H,
                                     double* x, double* y, double* z)
{
  constexpr double a = Airy1830_Ellipse.a;
  constexpr double b = Airy1830_Ellipse.b();

  GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b);

  return;
}



/* @func GPS_Math_WGS84LatLonH_To_XYZ **********************************
**
** Convert WGS84 latitude and longitude and ellipsoidal height to
** X, Y & Z coordinates
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] H [double] ellipsoidal height (metres)
** @param [w] x [double *] X
** @param [w] y [double *] Y
** @param [w] z [double *] Z
**
** @return [void]
************************************************************************/
void GPS_Math_WGS84LatLonH_To_XYZ(double phi, double lambda, double H,
                                  double* x, double* y, double* z)
{
  constexpr double a = WGS84_Ellipse.a;
  constexpr double b = WGS84_Ellipse.b();

  GPS_Math_LatLonH_To_XYZ(phi,lambda,H,x,y,z,a,b);

  return;
}




/* @func GPS_Math_XYZ_To_Airy1830LatLonH **********************************
**
** Convert XYZ to Airy 1830 latitude and longitude and ellipsoidal height
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] H [double] ellipsoidal height (metres)
** @param [w] x [double *] X
** @param [w] y [double *] Y
** @param [w] z [double *] Z
**
** @return [void]
************************************************************************/
void GPS_Math_XYZ_To_Airy1830LatLonH(double* phi, double* lambda, double* H,
                                     double x, double y, double z)
{
  constexpr double a = Airy1830_Ellipse.a;
  constexpr double b = Airy1830_Ellipse.b();

  GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b);

  return;
}



/* @func GPS_Math_XYZ_To_WGS84LatLonH **********************************
**
** Convert XYZ to WGS84 latitude and longitude and ellipsoidal height
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] H [double] ellipsoidal height (metres)
** @param [w] x [double *] X
** @param [w] y [double *] Y
** @param [w] z [double *] Z
**
** @return [void]
************************************************************************/
void GPS_Math_XYZ_To_WGS84LatLonH(double* phi, double* lambda, double* H,
                                  double x, double y, double z)
{
  constexpr double a = WGS84_Ellipse.a;
  constexpr double b = WGS84_Ellipse.b();

  GPS_Math_XYZ_To_LatLonH(phi,lambda,H,x,y,z,a,b);

  return;
}


/* @func  GPS_Math_LatLon_To_EN **********************************
**
** Convert latitude and longitude to eastings and northings
** Standard Gauss-Kruger Transverse Mercator
**
** @param [w] E [double *] easting (metres)
** @param [w] N [double *] northing (metres)
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [r] N0 [double] true northing origin (metres)
** @param [r] E0 [double] true easting  origin (metres)
** @param [r] phi0 [double] true latitude origin (deg)
** @param [r] lambda0 [double] true longitude origin (deg)
** @param [r] F0 [double] scale factor on central meridian
** @param [r] a [double] semi-major axis (metres)
** @param [r] b [double] semi-minor axis (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_LatLon_To_EN(double* E, double* N, double phi,
                           double lambda, double N0, double E0,
                           double phi0, double lambda0,
                           double F0, double a, double b)
{
  double esq;
  double n;
  double etasq;
  double nu;
  double rho;
  double M;
  double I;
  double II;
  double III;
  double IIIA;
  double IV;
  double V;
  double VI;

  double tmp;
  double tmp2;
  double fdf;
  double fde;

  phi0    = GPS_Math_Deg_To_Rad(phi0);
  lambda0 = GPS_Math_Deg_To_Rad(lambda0);
  phi     = GPS_Math_Deg_To_Rad(phi);
  lambda  = GPS_Math_Deg_To_Rad(lambda);

  esq = ((a*a)-(b*b)) / (a*a);
  n   = (a-b) / (a+b);

  tmp  = 1.0 - (esq * sin(phi) * sin(phi));
  nu   = a * F0 * pow(tmp,-0.5);
  rho  = a * F0 * (1.0 - esq) * pow(tmp,-1.5);
  etasq = (nu / rho) - 1.0;

  fdf   = 5.0 / 4.0;
  tmp   = 1.0 + n + (fdf * n * n) + (fdf * n * n * n);
  tmp  *= (phi - phi0);
  tmp2  = 3.0*n + 3.0*n*n + (21./8.)*n*n*n;
  tmp2 *= (sin(phi-phi0) * cos(phi+phi0));
  tmp  -= tmp2;

  fde   = (15.0 / 8.0);
  tmp2  = ((fde*n*n) + (fde*n*n*n)) * sin(2.0 * (phi-phi0));
  tmp2 *= cos(2.0 * (phi+phi0));
  tmp  += tmp2;

  tmp2  = (35.0/24.0) * n * n * n;
  tmp2 *= sin(3.0 * (phi-phi0));
  tmp2 *= cos(3.0 * (phi+phi0));
  tmp  -= tmp2;

  M     = b * F0 * tmp;
  I     = M + N0;
  II    = (nu / 2.0) * sin(phi) * cos(phi);
  III   = (nu / 24.0) * sin(phi) * cos(phi) * cos(phi) * cos(phi);
  III  *= (5.0 - (tan(phi) * tan(phi)) + (9.0 * etasq));
  IIIA  = (nu / 720.0) * sin(phi) * pow(cos(phi),5.0);
  IIIA *= (61.0 - (58.0*tan(phi)*tan(phi)) +
           pow(tan(phi),4.0));
  IV    = nu * cos(phi);

  tmp   = pow(cos(phi),3.0);
  tmp  *= ((nu/rho) - tan(phi) * tan(phi));
  V     = (nu/6.0) * tmp;

  tmp   = 5.0 - (18.0 * tan(phi) * tan(phi));
  tmp  += tan(phi)*tan(phi)*tan(phi)*tan(phi) + (14.0 * etasq);
  tmp  -= (58.0 * tan(phi) * tan(phi) * etasq);
  tmp2  = cos(phi)*cos(phi)*cos(phi)*cos(phi)*cos(phi) * tmp;
  VI    = (nu / 120.0) * tmp2;

  *N = I + II*(lambda-lambda0)*(lambda-lambda0) +
       III*pow((lambda-lambda0),4.0) +
       IIIA*pow((lambda-lambda0),6.0);

  *E = E0 + IV*(lambda-lambda0) + V*pow((lambda-lambda0),3.0) +
       VI * pow((lambda-lambda0),5.0);

  return;
}



/* @func GPS_Math_Airy1830MLatLonToINGEN ************************************
**
** Convert Modified Airy 1830  datum latitude and longitude to Irish
** National Grid Eastings and Northings
**
** @param [r] phi [double] modified Airy latitude     (deg)
** @param [r] lambda [double] modified Airy longitude (deg)
** @param [w] E [double *] NG easting (metres)
** @param [w] N [double *] NG northing (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_Airy1830M_LatLonToINGEN(double phi, double lambda, double* E,
                                      double* N)
{
  constexpr double N0      =  250000;
  constexpr double E0      =  200000;
  constexpr double F0      = 1.000035;
  constexpr double phi0    = 53.5;
  constexpr double lambda0 = -8.;
  constexpr double a       = Airy1830Modified_Ellipse.a;
  constexpr double b       = Airy1830Modified_Ellipse.b();

  GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);

  return;
}




/* @func GPS_Math_Airy1830LatLonToNGEN **************************************
**
** Convert Airy 1830 datum latitude and longitude to UK Ordnance Survey
** National Grid Eastings and Northings
**
** @param [r] phi [double] WGS84 latitude     (deg)
** @param [r] lambda [double] WGS84 longitude (deg)
** @param [w] E [double *] NG easting (metres)
** @param [w] N [double *] NG northing (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_Airy1830LatLonToNGEN(double phi, double lambda, double* E,
                                   double* N)
{
  constexpr double N0      = -100000;
  constexpr double E0      =  400000;
  constexpr double F0      = 0.9996012717;
  constexpr double phi0    = 49.;
  constexpr double lambda0 = -2.;
  constexpr double a       = Airy1830_Ellipse.a;
  constexpr double b       = Airy1830_Ellipse.b();

  GPS_Math_LatLon_To_EN(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);

  return;
}


/* @func int32 GPS_Math_WGS84_To_Swiss_EN ******************************
**
** Convert WGS84 latitude and longitude to
** Swiss CH-1903 National Grid Eastings and Northings
** ( Oblique Mercator Projection )
**
** @param [r] phi [double] WGS84 latitude     (deg)
** @param [r] lambda [double] WGS84 longitude (deg)
** @param [w] E [double *] Swiss-NG easting (metres)
** @param [w] N [double *] Swiss-NG northing (metres)
**
** @return [void]
************************************************************************/

int32_t GPS_Math_WGS84_To_Swiss_EN(double lat, double lon, double* E,
                                   double* N)
{
  constexpr double phi0 = 46.95240556;
  constexpr double lambda0 = 7.43958333;
  constexpr double E0 = 600000.0;
  constexpr double N0 = 200000.0;
  double phi, lambda, alt;

  if (lat < 44.89022757) {
    return 0;
  }
  if (lon < -0.16386312) {
    return 0;
  }

  constexpr double a = Bessel1841_Ellipse.a;
  constexpr double b = Bessel1841_Ellipse.b();

  GPS_Math_WGS84_To_Known_Datum_M(lat, lon, 0, &phi, &lambda, &alt, 123);
  GPS_Math_Swiss_LatLon_To_EN(phi, lambda, E, N, phi0, lambda0, E0, N0, a, b);

  return 1;
}


/* @GPS_Math_Swiss_EN_To_WGS84 *****************************************
**
** Convert WGS84 latitude and longitude to
** Swiss CH-1903 National Grid Eastings and Northings
**
** @param [r] E [double] Swiss-NG easting (metres)
** @param [r] N [double] Swiss-NG northing (metres)
** @param [w] lat [double *] WGS84 latitude     (deg)
** @param [w] lon [double *] WGS84 longitude (deg)
**
** @return [void]
************************************************************************/
void GPS_Math_Swiss_EN_To_WGS84(double E, double N, double* lat, double* lon)
{
  constexpr double phi0 = 46.95240556;
  constexpr double lambda0 = 7.43958333;
  constexpr double E0 = 600000.0;
  constexpr double N0 = 200000.0;
  double phi, lambda, alt;

  constexpr double a = Bessel1841_Ellipse.a;
  constexpr double b = Bessel1841_Ellipse.b();

  GPS_Math_Swiss_EN_To_LatLon(E, N, &phi, &lambda, phi0, lambda0, E0, N0, a, b);
  GPS_Math_Known_Datum_To_WGS84_M(phi, lambda, 0, lat, lon, &alt, 123);
}


/* @func GPS_Math_Cassini_LatLon_To_EN **********************************
**
** Convert latitude and longitude to Cassini transverse cylindrical projection
**  easting and northing
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [w] E [double *] easting (metre)
** @param [w] N [double *] northing (metre)
** @param [r] phi0 [double] latitude of origin (deg)
** @param [r] M0 [double] central meridian (deg)
** @param [r] E0 [double] false easting
** @param [r] N0 [double] false northing
** @param [r] a [double] semi-major axis
** @param [r] b [double] semi-minor axis
**
** @return [void]
************************************************************************/
void GPS_Math_Cassini_LatLon_To_EN(double phi, double lambda, double* E,
                                   double* N, double phi0, double M0,
                                   double E0, double N0, double a, double b)
{
  double p2;
  double a2;
  double b2;
  double e2;
  double e4;
  double e6;
  double AM0;
  double c0;
  double c1;
  double c2;
  double c3;
  double om0;
#if 0
  double A0;
  double A1;
  double A2;
  double A3;
#endif
  double j;
  double te4;
  double phi0s2;
  double phi0s4;
  double phi0s6;
  double lat;
#if 0
  double x;
  double E1;
  double E2;
  double E3;
  double E4;
#endif

  double phis;
  double phic;
  double phit;
  double phis2;
  double phis4;
  double phis6;
  double RD;
  double dlam;
  double NN;
  double TT;
  double WW;
  double WW2;
  double WW3;
  double WW4;
  double WW5;
  double CC;
  double MM;


  lambda = GPS_Math_Deg_To_Rad(lambda);
  phi0   = GPS_Math_Deg_To_Rad(phi0);
  phi    = GPS_Math_Deg_To_Rad(phi);
  M0     = GPS_Math_Deg_To_Rad(M0);


  p2 = GPS_PI * 2.;

  a2 = a*a;
  b2 = b*b;
  e2 = (a2-b2)/a2;
  e4 = e2*e2;
  e6 = e2*e4;

  te4 = 3.0 * e4;
  j   = 45. * e6 / 1024.;
  c0 = 1.0-e2/4.-te4/64.-5.*e6/256.;
  c1 = 3.*e2/8.+te4/32.+j;
  c2 = 15.*e4/256.+j;
  c3 = 35.*e6/3072.;

  lat = c0*phi0;
  phi0s2 = c1 * sin(2.*phi0);
  phi0s4 = c2 * sin(4.*phi0);
  phi0s6 = c3 * sin(6.*phi0);
  AM0 = a * (lat-phi0s2+phi0s4-phi0s6);

  om0 = 1.0 - e2;
#if 0
  // None of this is used -- is there a reason to keep it?
  x = pow(om0,(double)0.5);
  E1 = ((double)1.0 - x) / ((double)1.0 + x);
  E2 = E1*E1;
  E3 = E1*E2;
  E4 = E1*E3;
  A0 = (double)3.*E1/(double)2.-(double)27.*E3/(double)32.;
  A1 = (double)21.*E2/(double)16.-(double)55.*E4/(double)32.;
  A2 = (double)151.*E3/(double)96.;
  A3 = (double)1097.*E4/(double)512.;
#endif


  dlam = lambda - M0;
  if (dlam>GPS_PI) {
    dlam -= p2;
  }
  if (dlam<-GPS_PI) {
    dlam += p2;
  }

  phis = sin(phi);
  phic = cos(phi);
  phit = tan(phi);
  RD = pow(1.-e2*phis*phis,.5);
  NN = a/RD;
  TT = phit*phit;
  WW = dlam*phic;
  WW2 = WW*WW;
  WW3 = WW*WW2;
  WW4 = WW*WW3;
  WW5 = WW*WW4;
  CC = e2*phic*phic/om0;
  lat = c0*phi;
  phis2 = c1 * sin(2.*phi);
  phis4 = c2 * sin(4.*phi);
  phis6 = c3 * sin(6.*phi);
  MM = a * (lat-phis2+phis4-phis6);

  *E = NN*(WW-(TT*WW3/6.)-(8.-TT+8.*CC)*
           (TT*WW5/120.)) + E0;
  *N = MM-AM0+NN*phit*((WW2/2.)+(5.-TT+6.*CC) *
                       WW4/24.) + N0;
  return;
}




/* @func GPS_Math_Cassini_EN_To_LatLon **********************************
**
** Convert Cassini transverse cylindrical easting and northing projection
** to latitude and longitude
**
** @param [r] E [double] easting (metre)
** @param [r] N [double] northing (metre)
** @param [w] phi [double *] latitude (deg)
** @param [w] lambda [double *] longitude (deg)
** @param [r] phi0 [double] latitude of origin (deg)
** @param [r] M0 [double] central meridian (deg)
** @param [r] E0 [double] false easting
** @param [r] N0 [double] false northing
** @param [r] a [double] semi-major axis
** @param [r] b [double] semi-minor axis
**
** @return [void]
************************************************************************/
void GPS_Math_Cassini_EN_To_LatLon(double E, double N, double* phi,
                                   double* lambda, double phi0, double M0,
                                   double E0, double N0, double a, double b)

{
  double p2;
  double po2;
  double a2;
  double b2;
  double e2;
  double e4;
  double e6;
  double AM0;
  double c0;
  double c1;
  double c2;
  double c3;
  double om0;
  double A0;
  double A1;
  double A2;
  double A3;
  double j;
  double te4;
  double phi0s2;
  double phi0s4;
  double phi0s6;
  double lat;
  double x;
  double E1;
  double E2;
  double E3;
  double E4;

  double dx;
  double dy;
  double mu;
  double mus2;
  double mus4;
  double mus6;
  double mus8;
  double M1;
  double phi1;
  double phi1s;
  double phi1c;
  double phi1t;
  double T;
  double T1;
  double N1;
  double R1;
  double RD;
  double DD;
  double D2;
  double D3;
  double D4;
  double D5;
  double tol;

  M0 = GPS_Math_Deg_To_Rad(M0);
  phi0 = GPS_Math_Deg_To_Rad(phi0);

  p2 = GPS_PI * 2.;
  po2 = GPS_PI / 2.;

  a2 = a*a;
  b2 = b*b;
  e2 = (a2-b2)/a2;
  e4 = e2*e2;
  e6 = e2*e4;

  te4 = 3.0 * e4;
  j   = 45. * e6 / 1024.;
  c0 = 1.0-e2/4.-te4/64.-5.*e6/256.;
  c1 = 3.*e2/8.+te4/32.+j;
  c2 = 15.*e4/256.+j;
  c3 = 35.*e6/3072.;

  lat = c0*phi0;
  phi0s2 = c1 * sin(2.*phi0);
  phi0s4 = c2 * sin(4.*phi0);
  phi0s6 = c3 * sin(6.*phi0);
  AM0 = a * (lat-phi0s2+phi0s4-phi0s6);

  om0 = 1.0 - e2;
  x = pow(om0,0.5);
  E1 = (1.0 - x) / (1.0 + x);
  E2 = E1*E1;
  E3 = E1*E2;
  E4 = E1*E3;
  A0 = 3.*E1/2.-27.*E3/32.;
  A1 = 21.*E2/16.-55.*E4/32.;
  A2 = 151.*E3/96.;
  A3 = 1097.*E4/512.;



  tol = 1.e-5;

  dx = E - E0;
  dy = N - N0;
  M1 = AM0 + dy;
  mu = M1 / (a*c0);
  mus2 = A0 * sin(2.*mu);
  mus4 = A1 * sin(4.*mu);
  mus6 = A2 * sin(6.*mu);
  mus8 = A3 * sin(8.*mu);
  phi1 = mu + mus2 + mus4 + mus6 + mus8;

  if ((((po2-tol)<phi1)&&(phi1<(po2+tol)))) {
    *phi = po2;
    *lambda = M0;
  } else if ((((-po2-tol)<phi1)&&(phi1<(-po2+tol)))) {
    *phi = -po2;
    *lambda = M0;
  } else {
    phi1s = sin(phi1);
    phi1c = cos(phi1);
    phi1t = tan(phi1);
    T1 = phi1t*phi1t;
    RD = pow(1.-e2*phi1s*phi1s,.5);
    N1 = a/RD;
    R1 = N1 * om0 / (RD*RD);
    DD = dx/N1;
    D2 = DD*DD;
    D3 = DD*D2;
    D4 = DD*D3;
    D5 = DD*D4;
    T = 1. + 3.*T1;
    *phi = phi1-(N1*phi1t/R1)*(D2/2.-T*D4/24.);
    *lambda = M0+(DD-T1*D3/3.+T*T1*D5/15.)/phi1c;

    if (*phi>po2) {
      *phi=po2;
    } else if (*phi<-po2) {
      *phi=-po2;
    }

    if (*lambda>GPS_PI) {
      *lambda -= p2;
    }
    if (*lambda<-GPS_PI) {
      *lambda += p2;
    }

    if (*lambda>GPS_PI) {
      *lambda=GPS_PI;
    } else if (*lambda<-GPS_PI) {
      *lambda=-GPS_PI;
    }
  }

  *lambda = GPS_Math_Rad_To_Deg(*lambda);
  *phi    = GPS_Math_Rad_To_Deg(*phi);

  return;
}




/* @func int32 GPS_Math_WGS84_To_ICS_EN ******************************
**
** Convert WGS84 latitude and longitude to
** Israeli old  Grid Eastings and Northings
** ( Israeli Cassini Soldner )
**
** @param [r] phi [double] WGS84 latitude     (deg)
** @param [r] lambda [double] WGS84 longitude (deg)
** @param [w] E [double *] ICS easting (metres)
** @param [w] N [double *] ICS northing (metres)
**
** @return [void]
************************************************************************/

int32_t GPS_Math_WGS84_To_ICS_EN(double lat, double lon, double* E,
                                 double* N)
{
  constexpr double phi0    = 31.73409694444; // 31 44 2.749
  constexpr double lambda0 = 35.21208055556; // 35 12 43.49
  constexpr double E0      = 170251.555;
  constexpr double N0      = 1126867.909;
  double phi, lambda, alt, a, b;

  int32_t datum = GPS_Lookup_Datum_Index("Palestine 1923");
  if (datum < 0) {
    gbFatal("Unable to find Palestine 1923 in internal tables\n");
  }
  int32_t ellipse = GPS_Datums[datum].ellipse;

  a = GPS_Ellipses[ellipse].a;
  b = GPS_Ellipses[ellipse].b();

  GPS_Math_WGS84_To_Known_Datum_M(lat, lon, 0, &phi, &lambda, &alt, datum);
  GPS_Math_Cassini_LatLon_To_EN(phi, lambda, E, N,
                                phi0, lambda0, E0, N0, a, b);

  return 1;
}


/* @GPS_Math_ICS_EN_To_WGS84 *****************************************
**
** Convert WGS84 latitude and longitude to
** Israeli Old Grid Eastings and Northings
**
** @param [r] E [double] ICS easting (metres)
** @param [r] N [double] ICS northing (metres)
** @param [w] lat [double *] WGS84 latitude     (deg)
** @param [w] lon [double *] WGS84 longitude (deg)
**
** @return [void]
************************************************************************/
void GPS_Math_ICS_EN_To_WGS84(double E, double N, double* lat, double* lon)
{
  constexpr double phi0    = 31.73409694444; // 31 44 2.749
  constexpr double lambda0 = 35.21208055556; // 35 12 43.49
  constexpr double E0      = 170251.555;
  constexpr double N0      = 1126867.909;
  double phi, lambda, alt, a, b;
  int32_t datum = GPS_Lookup_Datum_Index("Palestine 1923");
  if (datum < 0) {
    gbFatal("Unable to find Palestine 1923 in internal tables\n");
  }
  int32_t ellipse = GPS_Datums[datum].ellipse;

  a = GPS_Ellipses[ellipse].a;
  b = GPS_Ellipses[ellipse].b();

  GPS_Math_Cassini_EN_To_LatLon(E, N, &phi, &lambda, phi0, lambda0,
                                E0, N0, a, b);
  GPS_Math_Known_Datum_To_WGS84_M(phi, lambda, 0, lat, lon, &alt, datum);
}



/* @func  GPS_Math_EN_To_LatLon **************************************
**
** Convert Eastings and Northings to latitude and longitude
**
** @param [w] E [double] NG easting (metres)
** @param [w] N [double] NG northing (metres)
** @param [r] phi [double *] Airy latitude     (deg)
** @param [r] lambda [double *] Airy longitude (deg)
** @param [r] N0 [double] true northing origin (metres)
** @param [r] E0 [double] true easting  origin (metres)
** @param [r] phi0 [double] true latitude origin (deg)
** @param [r] lambda0 [double] true longitude origin (deg)
** @param [r] F0 [double] scale factor on central meridian
** @param [r] a [double] semi-major axis (metres)
** @param [r] b [double] semi-minor axis (metres)
**
** @return [void]
************************************************************************/
void GPS_Math_EN_To_LatLon(double E, double N, double* phi,
                           double* lambda, double N0, double E0,
                           double phi0, double lambda0,
                           double F0, double a, double b)
{
  double esq;
  double n;
  double etasq;
  double nu;
  double rho;
  double M;
  double VII;
  double VIII;
  double IX;
  double X;
  double XI;
  double XII;
  double XIIA;
  double phix;
  double nphi=0.0;

  double tmp;
  double fdf;
  double fde;

  phi0    = GPS_Math_Deg_To_Rad(phi0);
  lambda0 = GPS_Math_Deg_To_Rad(lambda0);

  n     = (a-b) / (a+b);
  fdf   = 5.0 / 4.0;
  fde   = (15.0 / 8.0);

  esq = ((a*a)-(b*b)) / (a*a);


  phix = ((N-N0)/(a*F0)) + phi0;

  tmp  = 1.0 - (esq * sin(phix) * sin(phix));
  nu   = a * F0 * pow(tmp,-0.5);
  rho  = a * F0 * (1.0 - esq) * pow(tmp,-1.5);
  etasq = (nu / rho) - 1.0;

  M = -1e20;

  while (N-N0-M > 0.000001) {
    nphi = phix;

    tmp   = 1.0 + n + (fdf * n * n) + (fdf * n * n * n);
    tmp  *= (nphi - phi0);
    double tmp2  = 3.0*n + 3.0*n*n + (21./8.)*n*n*n;
    tmp2 *= (sin(nphi-phi0) * cos(nphi+phi0));
    tmp  -= tmp2;


    tmp2  = ((fde*n*n) + (fde*n*n*n)) * sin(2.0 * (nphi-phi0));
    tmp2 *= cos(2.0 * (nphi+phi0));
    tmp  += tmp2;

    tmp2  = (35.0/24.0) * n * n * n;
    tmp2 *= sin(3.0 * (nphi-phi0));
    tmp2 *= cos(3.0 * (nphi+phi0));
    tmp  -= tmp2;

    M     = b * F0 * tmp;

    if (N-N0-M > 0.000001) {
      phix = ((N-N0-M)/(a*F0)) + nphi;
    }
  }


  VII  = tan(nphi) / (2.0 * rho * nu);

  tmp  = 5.0 + 3.0 * tan(nphi) * tan(nphi) + etasq;
  tmp -= 9.0 * tan(nphi) * tan(nphi) * etasq;
  VIII = (tan(nphi)*tmp) / (24.0 * rho * nu*nu*nu);

  tmp  = 61.0 + 90.0 * tan(nphi) * tan(nphi);
  tmp += 45.0 * pow(tan(nphi),4.0);
  IX   = tan(nphi) / (720.0 * rho * pow(nu,5.0)) * tmp;

  X    = 1.0 / (cos(nphi) * nu);

  tmp  = (nu / rho) + 2.0 * tan(nphi) * tan(nphi);
  XI   = (1.0 / (cos(nphi) * 6.0 * nu*nu*nu)) * tmp;

  tmp  = 5.0 + 28.0 * tan(nphi)*tan(nphi);
  tmp += 24.0 * pow(tan(nphi),4.0);
  XII  = (1.0 / (120.0 * pow(nu,5.0) * cos(nphi)))
         * tmp;

  tmp  = 61.0 + 662.0 * tan(nphi) * tan(nphi);
  tmp += 1320.0 * pow(tan(nphi),4.0);
  tmp += 720.0  * pow(tan(nphi),6.0);
  XIIA = (1.0 / (cos(nphi) * 5040.0 * pow(nu,7.0)))
         * tmp;

  *phi = nphi - VII*pow((E-E0),2.0) + VIII*pow((E-E0),4.0) -
         IX*pow((E-E0),6.0);

  *lambda = lambda0 + X*(E-E0) - XI*pow((E-E0),3.0) +
            XII*pow((E-E0),5.0) - XIIA*pow((E-E0),7.0);

  *phi    = GPS_Math_Rad_To_Deg(*phi);
  *lambda = GPS_Math_Rad_To_Deg(*lambda);

  return;
}




/* @func GPS_Math_NGENToAiry1830LatLon **************************************
**
** Convert  to UK Ordnance Survey National Grid Eastings and Northings to
** Airy 1830 datum latitude and longitude
**
** @param [r] E [double] NG easting (metres)
** @param [r] N [double] NG northing (metres)
** @param [w] phi [double *] Airy latitude     (deg)
** @param [w] lambda [double *] Airy longitude (deg)
**
** @return [void]
************************************************************************/
void GPS_Math_NGENToAiry1830LatLon(double E, double N, double* phi,
                                   double* lambda)
{
  constexpr double N0      = -100000;
  constexpr double E0      =  400000;
  constexpr double F0      = 0.9996012717;
  constexpr double phi0    = 49.;
  constexpr double lambda0 = -2.;
  constexpr double a = Airy1830_Ellipse.a;
  constexpr double b = Airy1830_Ellipse.b();

  GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);

  return;
}



/* @func GPS_Math_INGENToAiry1830MLatLon **************************************
**
** Convert Irish National Grid Eastings and Northings to modified
** Airy 1830 datum latitude and longitude
**
** @param [r] E [double] ING easting (metres)
** @param [r] N [double] ING northing (metres)
** @param [w] phi [double *] modified Airy latitude     (deg)
** @param [w] lambda [double *] modified Airy longitude (deg)
**
** @return [void]
************************************************************************/
void GPS_Math_INGENToAiry1830MLatLon(double E, double N, double* phi,
                                     double* lambda)
{
  constexpr double N0      =  250000;
  constexpr double E0      =  200000;
  constexpr double F0      = 1.000035;
  constexpr double phi0    = 53.5;
  constexpr double lambda0 = -8.;
  constexpr double a = Airy1830Modified_Ellipse.a;
  constexpr double b = Airy1830Modified_Ellipse.b();

  GPS_Math_EN_To_LatLon(E,N,phi,lambda,N0,E0,phi0,lambda0,F0,a,b);

  return;
}



/* @func GPS_Math_EN_To_UKOSNG_Map *************************************
**
** Convert Airy 1830 eastings and northings to Ordnance Survey map
** two letter code plus modified eastings and northings
**
** @param [r] E [double] NG easting (metres)
** @param [r] N [double] NG northing (metres)
** @param [w] mE [double *] modified easting (metres)
** @param [w] mN [double *] modified northing (metres)
** @param [w] map [char *] map code
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_EN_To_UKOSNG_Map(double E, double N, double* mE,
                                  double* mN, char* map)
{
	int32_t t;
	int32_t idx;

  if (E>=700000. || E<0.0 || N<0.0 ||
      N>=1300000.0) {
    return 0;
  }

  idx = ((int32_t)N/100000)*7 + (int32_t)E/100000;
  (void) strcpy(map,UKNG[idx]);

  t = ((int32_t)E / 100000) * 100000;
  *mE = E - (double)t;

  t = ((int32_t)N / 100000) * 100000;
  *mN = N - (double)t;

  return 1;
}



/* @func GPS_Math_UKOSNG_Map_To_EN *************************************
**
** Convert Ordnance Survey map eastings and northings plus
** two letter code to Airy 1830 eastings and northings
**
** @param [w] map [const char *] map code
** @param [r] mapE [double] easting (metres)
** @param [r] mapN [double] northing (metres)
** @param [w] E [double *] full Airy easting (metres)
** @param [w] N [double *] full Airy northing (metres)

**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UKOSNG_Map_To_EN(const char* map, double mapE, double mapN,
                                  double* E, double* N)
{
	int32_t t;
	int32_t idx;

  if (mapE>=100000.0 || mapE<0.0 || mapN<0.0 ||
      mapN>100000.0) {
    return 0;
  }

  idx=0;
  while (*UKNG[idx]) {
    if (!strcmp(UKNG[idx],map)) {
      break;
    }
    ++idx;
  }
  if (!*UKNG[idx]) {
    return 0;
  }


  t = (idx / 7) * 100000;
  *N = mapN + (double)t;

  t = (idx % 7) * 100000;
  *E = mapE + (double)t;

  return 1;
}



/* @func GPS_Math_Molodensky *******************************************
**
** Transform one datum to another
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [r] Sa   [double] source semi-major axis (metres)
** @param [r] Sif  [double] source inverse flattening
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] Da   [double]   dest semi-major axis (metres)
** @param [r] Dif  [double]   dest inverse flattening
** @param [r] dx  [double]   dx
** @param [r] dy  [double]   dy
** @param [r] dz  [double]   dz
**
** @return [void]
************************************************************************/
void GPS_Math_Molodensky(double Sphi, double Slam, double SH, double Sa,
                         double Sif, double* Dphi, double* Dlam,
                         double* DH, double Da, double Dif, double dx,
                         double dy, double dz)
{
  double Sf;
  double Df;
  double esq;
  double bda;
  double da;
  double df;
  double N;
  double M;
  double tmp;
  double tmp2;
  double dphi;
  double dlambda;
  double dheight;
  double phis;
  double phic;
  double lams;
  double lamc;

  Sf = 1.0 / Sif;
  Df = 1.0 / Dif;

  esq = 2.0*Sf - pow(Sf,2.0);
  bda = 1.0 - Sf;
  Sphi = GPS_Math_Deg_To_Rad(Sphi);
  Slam = GPS_Math_Deg_To_Rad(Slam);

  da = Da - Sa;
  df = Df - Sf;

  phis = sin(Sphi);
  phic = cos(Sphi);
  lams = sin(Slam);
  lamc = cos(Slam);

  N = Sa /  sqrt(1.0 - esq*pow(phis,2.0));

  tmp = (1.0-esq) /pow((1.0-esq*pow(phis,2.0)),1.5);
  M   = Sa * tmp;

  tmp  = df * ((M/bda)+N*bda) * phis * phic;
  tmp2 = da * N * esq * phis * phic / Sa;
  tmp2 += ((-dx*phis*lamc-dy*phis*lams) + dz*phic);
  dphi = (tmp2 + tmp) / (M + SH);

  dlambda = (-dx*lams+dy*lamc) / ((N+SH)*phic);

  dheight = dx*phic*lamc + dy*phic*lams + dz*phis - da*(Sa/N) +
            df*bda*N*phis*phis;

  *Dphi = Sphi + dphi;
  *Dlam = Slam + dlambda;
  *DH   = SH   + dheight;

  *Dphi = GPS_Math_Rad_To_Deg(*Dphi);
  *Dlam = GPS_Math_Rad_To_Deg(*Dlam);

  return;
}


/* @func GPS_Math_Helmert *******************************************
**
** Transform one datum to another
**
** @param [r] Sx [double] source X
** @param [r] Sy [double] source Y
** @param [r] Sz [double] source Z
** @param [w] Dx [double*] converted X
** @param [w] Dy [double*] converted Y
** @param [w] Dz [double*] converted Z
** @param [r] tX   [double] translation along x axis (meters)
** @param [r] tY   [double] translation along y axis (meters)
** @param [r] tZ   [double] translation along z axis (meters)
** @param [r] sppm [double] scale factor - 1 (ppm)
** @param [r] rXs  [double] rotation about x axis (seconds)
** @param [r] rYs  [double] rotation about y axis (seconds)
** @param [r] rZs  [double] rotation about z axis (seconds)
**
** @return [void]
************************************************************************/
void GPS_Math_Helmert(double Sx, double Sy, double Sz,
                      double* Dx, double* Dy, double* Dz,
                      double tX, double tY, double tZ,
                      double sppm,
                      double rXs, double rYs, double rZs)
{
  double s = sppm * 1e-6;
  constexpr double radians_per_second = (1.0/(60.0*60.0)) * GPS_PI/180.0;
  double rX = rXs * radians_per_second; /* radians */
  double rY = rYs * radians_per_second; /* radians */
  double rZ = rZs * radians_per_second; /* radians */

  // rotation/scaling matrix = [1+s -rz ry;rz 1+s -rx;-ry rx 1+s];
  *Dx = tX + (1 + s)*Sx + -rZ*Sy + rY*Sz;
  *Dy = tY + rZ*Sx + (1 + s)*Sy + -rX*Sz;
  *Dz = tZ + -rY*Sx + rX*Sy + (1 + s)*Sz;
}

/* @func GPS_Math_Inverse_Helmert ***********************************
**
** Transform from source to coverted datum.
** translation, scale and rotation parameters are the Helmert parameters
** to traslate from the converted to source datum!
**
** @param [r] Sx [double] source X
** @param [r] Sy [double] source Y
** @param [r] Sz [double] source Z
** @param [w] Dx [double*] converted X
** @param [w] Dy [double*] converted Y
** @param [w] Dz [double*] converted Z
** @param [r] tX   [double] translation along x axis (meters)
** @param [r] tY   [double] translation along y axis (meters)
** @param [r] tZ   [double] translation along z axis (meters)
** @param [r] sppm [double] scale factor - 1 (ppm)
** @param [r] rXs  [double] rotation about x axis (seconds)
** @param [r] rYs  [double] rotation about y axis (seconds)
** @param [r] rZs  [double] rotation about z axis (seconds)
**
** @return [void]
************************************************************************/
void GPS_Math_Inverse_Helmert(double Sx, double Sy, double Sz,
                              double* Dx, double* Dy, double* Dz,
                              double tX, double tY, double tZ,
                              double sppm,
                              double rXs, double rYs, double rZs)
{
  double s = sppm * 1e-6;
  constexpr double radians_per_second = (1.0/(60.0*60.0)) * GPS_PI/180.0;
  double rX = rXs * radians_per_second; /* radians */
  double rY = rYs * radians_per_second; /* radians */
  double rZ = rZs * radians_per_second; /* radians */

  // forward rotation/scaling matrix is [1+s -rz ry;rz 1+s -rx;-ry rx 1+s]
  // compute inverse of forward Helmert rotation/scaling matrix
  // https://www.wolframalpha.com/input?i2d=true&i=Power%5B%7B%7B1%2Bs%2C-Subscript%5Br%2Cz%5D%2CSubscript%5Br%2Cy%5D%7D%2C%7BSubscript%5Br%2Cz%5D%2C1%2Bs%2C-Subscript%5Br%2Cx%5D%7D%2C%7B-Subscript%5Br%2Cy%5D%2CSubscript%5Br%2Cx%5D%2C1%2Bs%7D%7D%2C-1%5D
  double gain = 1/((s + 1) * (rX*rX + rY*rY + rZ*rZ + s*s + 2*s + 1));
  double r11 = rX*rX + s*s + 2*s + 1;
  double r12 = s*rZ + rX*rY + rZ;
  double r13 = -s*rY + rX*rZ - rY;
  double r21 = -s*rZ + rX*rY - rZ;
  double r22 = rY*rY + s*s + 2*s + 1;
  double r23 = s*rX + rX + rY*rZ;
  double r31 = s*rY + rX*rZ + rY;
  double r32 = -s*rX - rX + rY*rZ;
  double r33 = rZ*rZ + s*s + 2*s + 1;

  *Dx = gain * ((r11 * (Sx-tX)) + (r12 * (Sy-tY)) + (r13 * (Sz-tZ)));
  *Dy = gain * ((r21 * (Sx-tX)) + (r22 * (Sy-tY)) + (r23 * (Sz-tZ)));
  *Dz = gain * ((r31 * (Sx-tX)) + (r32 * (Sy-tY)) + (r33 * (Sz-tZ)));
}

/* @func GPS_Math_Known_Datum_To_WGS84_M **********************************
**
** Transform datum to WGS84 using Molodensky
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_Known_Datum_To_WGS84_M(double Sphi, double Slam, double SH,
                                     double* Dphi, double* Dlam, double* DH,
                                     int32_t n)
{
  double Sa;
  double Sif;
  double x;
  double y;
  double z;
  int32_t idx;

  constexpr double Da  = WGS84_Ellipse.a;
  constexpr double Dif = WGS84_Ellipse.invf;

  idx  = GPS_Datums[n].ellipse;
  Sa   = GPS_Ellipses[idx].a;
  Sif  = GPS_Ellipses[idx].invf;
  x    = GPS_Datums[n].dx;
  y    = GPS_Datums[n].dy;
  z    = GPS_Datums[n].dz;

  GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);

  return;
}



/* @func GPS_Math_WGS84_To_Known_Datum_M ********************************
**
** Transform WGS84 to other datum using Molodensky
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_WGS84_To_Known_Datum_M(double Sphi, double Slam, double SH,
                                     double* Dphi, double* Dlam, double* DH,
                                     int32_t n)
{
  double Da;
  double Dif;
  double x;
  double y;
  double z;
  int32_t idx;

  constexpr double Sa  = WGS84_Ellipse.a;
  constexpr double Sif = WGS84_Ellipse.invf;

  idx  = GPS_Datums[n].ellipse;
  Da   = GPS_Ellipses[idx].a;
  Dif  = GPS_Ellipses[idx].invf;
  x    = -GPS_Datums[n].dx;
  y    = -GPS_Datums[n].dy;
  z    = -GPS_Datums[n].dz;

  GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);

  return;
}



/* @func GPS_Math_Known_Datum_To_WGS84_C **********************************
**
** Transform datum to WGS84 using Cartesian coordinates
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_Known_Datum_To_WGS84_C(double Sphi, double Slam, double SH,
                                     double* Dphi, double* Dlam, double* DH,
                                     int32_t n)
{
  double Sa;
  double Sif;
  double Sb;
  double x;
  double y;
  double z;
  int32_t idx;
  double sx;
  double sy;
  double sz;

  constexpr double Da  = WGS84_Ellipse.a;
  constexpr double Db  = WGS84_Ellipse.b();

  idx  = GPS_Datums[n].ellipse;
  Sa   = GPS_Ellipses[idx].a;
  Sif  = GPS_Ellipses[idx].invf;
  Sb   = Sa - (Sa / Sif);

  x    = GPS_Datums[n].dx;
  y    = GPS_Datums[n].dy;
  z    = GPS_Datums[n].dz;

  GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&sx,&sy,&sz,Sa,Sb);
  sx += x;
  sy += y;
  sz += z;

  GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,sx,sy,sz,Da,Db);

  return;
}



/* @func GPS_Math_WGS84_To_Known_Datum_C ********************************
**
** Transform WGS84 to other datum using Cartesian coordinates
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_WGS84_To_Known_Datum_C(double Sphi, double Slam, double SH,
                                     double* Dphi, double* Dlam, double* DH,
                                     int32_t n)
{
  double Da;
  double Dif;
  double x;
  double y;
  double z;
  int32_t idx;
  double Db;
  double dx;
  double dy;
  double dz;

  constexpr double Sa  = WGS84_Ellipse.a;
  constexpr double Sb  = WGS84_Ellipse.b();

  idx  = GPS_Datums[n].ellipse;
  Da   = GPS_Ellipses[idx].a;
  Dif  = GPS_Ellipses[idx].invf;
  Db  = Da - (Da / Dif);

  x    = -GPS_Datums[n].dx;
  y    = -GPS_Datums[n].dy;
  z    = -GPS_Datums[n].dz;

  GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb);
  dx += x;
  dy += y;
  dz += z;

  GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db);

  return;
}



/* @func GPS_Math_Known_Datum_To_Known_Datum_M *************************
**
** Transform WGS84 to other datum using Molodensky
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n1   [int32] source datum number from GPS_Datums structure
** @param [r] n2   [int32] dest   datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_Known_Datum_To_Known_Datum_M(double Sphi, double Slam, double SH,
    double* Dphi, double* Dlam,
    double* DH, int32_t n1, int32_t n2)
{
  double Sa;
  double Sif;
  double Da;
  double Dif;
  double x1;
  double y1;
  double z1;
  double x2;
  double y2;
  double z2;
  double x;
  double y;
  double z;

  int32_t idx1;
  int32_t idx2;


  idx1 = GPS_Datums[n1].ellipse;
  Sa   = GPS_Ellipses[idx1].a;
  Sif  = GPS_Ellipses[idx1].invf;
  x1   = GPS_Datums[n1].dx;
  y1   = GPS_Datums[n1].dy;
  z1   = GPS_Datums[n1].dz;

  idx2 = GPS_Datums[n2].ellipse;
  Da   = GPS_Ellipses[idx2].a;
  Dif  = GPS_Ellipses[idx2].invf;
  x2   = GPS_Datums[n2].dx;
  y2   = GPS_Datums[n2].dy;
  z2   = GPS_Datums[n2].dz;

  x = -(x2-x1);
  y = -(y2-y1);
  z = -(z2-z1);

  GPS_Math_Molodensky(Sphi,Slam,SH,Sa,Sif,Dphi,Dlam,DH,Da,Dif,x,y,z);

  return;
}



/* @func GPS_Math_Known_Datum_To_Known_Datum_C *************************
**
** Transform known datum to other datum using Cartesian coordinates
**
** @param [r] Sphi [double] source latitude (deg)
** @param [r] Slam [double] source longitude (deg)
** @param [r] SH   [double] source height  (metres)
** @param [w] Dphi [double *] dest latitude (deg)
** @param [w] Dlam [double *] dest longitude (deg)
** @param [w] DH   [double *] dest height  (metres)
** @param [r] n1   [int32] source datum number from GPS_Datums structure
** @param [r] n2   [int32] dest   datum number from GPS_Datums structure
**
** @return [void]
************************************************************************/
void GPS_Math_Known_Datum_To_Known_Datum_C(double Sphi, double Slam, double SH,
    double* Dphi, double* Dlam,
    double* DH, int32_t n1, int32_t n2)
{
  double Sa;
  double Da;
  double x1;
  double y1;
  double z1;
  double x2;
  double y2;
  double z2;

  int32_t idx1;
  int32_t idx2;

  double Sb;
  double Db;
  double dx;
  double dy;
  double dz;

  idx1  = GPS_Datums[n1].ellipse;
  Sa    = GPS_Ellipses[idx1].a;
  Sb    = GPS_Ellipses[idx1].b();

  x1    = GPS_Datums[n1].dx;
  y1    = GPS_Datums[n1].dy;
  z1    = GPS_Datums[n1].dz;

  idx2  = GPS_Datums[n2].ellipse;
  Da    = GPS_Ellipses[idx2].a;
  Db    = GPS_Ellipses[idx2].b();

  x2    = GPS_Datums[n2].dx;
  y2    = GPS_Datums[n2].dy;
  z2    = GPS_Datums[n2].dz;

  GPS_Math_LatLonH_To_XYZ(Sphi,Slam,SH,&dx,&dy,&dz,Sa,Sb);
  dx += -(x2-x1);
  dy += -(y2-y1);
  dz += -(z2-z1);

  GPS_Math_XYZ_To_LatLonH(Dphi,Dlam,DH,dx,dy,dz,Da,Db);

  return;
}



/* @func GPS_Math_WGS84_To_UKOSMap_M ***********************************
**
** Convert WGS84 lat/lon to Ordnance survey map code and easting and
** northing. Uses Molodensky
**
** @param [r] lat  [double] WGS84 latitude (deg)
** @param [r] lon  [double] WGS84 longitude (deg)
** @param [w] mE   [double *] map easting (metres)
** @param [w] mN   [double *] map northing (metres)
** @param [w] map  [char *] map two letter code
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_WGS84_To_UKOSMap_M(double lat, double lon, double* mE,
                                    double* mN, char* map)
{
  double alat;
  double alon;
  double aht;
  double aE;
  double aN;


  GPS_Math_WGS84_To_Known_Datum_M(lat,lon,30,&alat,&alon,&aht,86);

  GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN);

  if (!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) {
    return 0;
  }

  return 1;
}



/* @func GPS_Math_UKOSMap_To_WGS84_M ***********************************
**
** Transform UK Ordnance survey map position to WGS84 lat/lon
** Uses Molodensky transformation
**
** @param [r] map  [const char *] map two letter code
** @param [r] mE   [double] map easting (metres)
** @param [r] mN   [double] map northing (metres)
** @param [w] lat  [double *] WGS84 latitude (deg)
** @param [w] lon  [double *] WGS84 longitude (deg)
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UKOSMap_To_WGS84_M(const char* map, double mE, double mN,
                                    double* lat, double* lon)
{
  double E;
  double N;
  double alat;
  double alon;
  double ht;

  if (!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) {
    return 0;
  }

  GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon);

  GPS_Math_Known_Datum_To_WGS84_M(alat,alon,0,lat,lon,&ht,86);

  return 1;
}


/* @func GPS_Math_WGS84_To_UKOSMap_H ***********************************
**
** Convert WGS84 lat/lon to Ordnance survey map code and easting and
** northing. Uses Helmert.
**
** @param [r] lat  [double] WGS84 latitude (deg)
** @param [r] lon  [double] WGS84 longitude (deg)
** @param [w] mE   [double *] map easting (metres)
** @param [w] mN   [double *] map northing (metres)
** @param [w] map  [char *] map two letter code
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_WGS84_To_UKOSMap_H(double lat, double lon, double* mE,
                                    double* mN, char* map)
{
  double x;
  double y;
  double z;
  double ax;
  double ay;
  double az;
  double alat;
  double alon;
  double ah;
  double aE;
  double aN;


  GPS_Math_WGS84LatLonH_To_XYZ(lat, lon, 0,
                               &x, &y, &z);

  /* Helmert transformation
   * https://www.ordnancesurvey.co.uk/documents/resources/guide-coordinate-systems-great-britain.pdf
   * 6.2 Helmert datum transformations
   * 6.6 Approximate WGS84 to OSGB36/ODN transformation
   */
  // WGS84 -> OSGB36
  constexpr double tX = -446.448; /* meters */
  constexpr double tY = +125.157; /* meters */
  constexpr double tZ = -542.060; /* meters */
  constexpr double s = +20.4894; /* ppm */
  constexpr double rX = -0.1502; /* seconds */
  constexpr double rY = -0.2470; /* seconds */
  constexpr double rZ = -0.8421; /* seconds */

  GPS_Math_Helmert(x, y, z,
                   &ax, &ay, &az,
                   tX, tY, tZ,
                   s,
                   rX, rY, rZ);

  GPS_Math_XYZ_To_Airy1830LatLonH(&alat, &alon, &ah,
                                  ax, ay, az);
  GPS_Math_Airy1830LatLonToNGEN(alat, alon,
                                &aE, &aN);
  if (!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) {
    return 0;
  }

  return 1;
}


/* @func GPS_Math_UKOSMap_To_WGS84_H ***********************************
**
** Transform UK Ordnance survey map position to WGS84 lat/lon
** Uses Helmert transformation
**
** @param [r] map  [const char *] map two letter code
** @param [r] mE   [double] map easting (metres)
** @param [r] mN   [double] map northing (metres)
** @param [w] lat  [double *] WGS84 latitude (deg)
** @param [w] lon  [double *] WGS84 longitude (deg)
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UKOSMap_To_WGS84_H(const char* map, double mE, double mN,
                                    double* lat, double* lon)
{
  double E;
  double N;
  double alat;
  double alon;
  double ht;
  double ax;
  double ay;
  double az;
  double x;
  double y;
  double z;

  if (!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) {
    return 0;
  }

  GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon);
  GPS_Math_Airy1830LatLonH_To_XYZ(alat, alon, 0.0, &ax, &ay, &az);

  /* Helmert transformation
   * https://www.ordnancesurvey.co.uk/documents/resources/guide-coordinate-systems-great-britain.pdf
   * 6.2 Helmert datum transformations
   * 6.6 Approximate WGS84 to OSGB36/ODN transformation
   */
  if constexpr(use_exact_helmert_inverse) {
    // Actually invert the Helmert transform from WGS84 to OSGB36.
    // WGS84 -> OSGB36
    constexpr double tX = -446.448; /* meters */
    constexpr double tY = +125.157; /* meters */
    constexpr double tZ = -542.060; /* meters */
    constexpr double s = +20.4894; /* ppm */
    constexpr double rX = -0.1502; /* seconds */
    constexpr double rY = -0.2470; /* seconds */
    constexpr double rZ = -0.8421; /* seconds */

    GPS_Math_Inverse_Helmert(ax, ay, az,
                             &x, &y, &z,
                             tX, tY, tZ,
                             s,
                             rX, rY, rZ);
  } else {
    // Approximate the transform from OSGB36 to WGS84 by using the standard
    // helmert transform with parameters that all have the opposite signs of
    // those used to transform from WGS84 to OSGB36.
    constexpr double tX = +446.448; /* meters */
    constexpr double tY = -125.157; /* meters */
    constexpr double tZ = +542.060; /* meters */
    constexpr double s = -20.4894; /* ppm */
    constexpr double rX = +0.1502; /* seconds */
    constexpr double rY = +0.2470; /* seconds */
    constexpr double rZ = +0.8421; /* seconds */

    GPS_Math_Helmert(ax, ay, az,
                     &x, &y, &z,
                     tX, tY, tZ,
                     s,
                     rX, rY, rZ);
  }

  GPS_Math_XYZ_To_WGS84LatLonH(lat, lon, &ht, x, y, z);

  return 1;
}


/* @func GPS_Math_WGS84_To_UKOSMap_C ***********************************
**
** Convert WGS84 lat/lon to Ordnance survey map code and easting and
** northing. Uses cartesian transformation
**
** @param [r] lat  [double] WGS84 latitude (deg)
** @param [r] lon  [double] WGS84 longitude (deg)
** @param [w] mE   [double *] map easting (metres)
** @param [w] mN   [double *] map northing (metres)
** @param [w] map  [char *] map two letter code
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_WGS84_To_UKOSMap_C(double lat, double lon, double* mE,
                                    double* mN, char* map)
{
  double alat;
  double alon;
  double aht;
  double aE;
  double aN;


  GPS_Math_WGS84_To_Known_Datum_C(lat,lon,30,&alat,&alon,&aht,86);

  GPS_Math_Airy1830LatLonToNGEN(alat,alon,&aE,&aN);

  if (!GPS_Math_EN_To_UKOSNG_Map(aE,aN,mE,mN,map)) {
    return 0;
  }

  return 1;
}



/* @func GPS_Math_UKOSMap_To_WGS84_C ***********************************
**
** Transform UK Ordnance survey map position to WGS84 lat/lon
** Uses cartesian transformation
**
** @param [r] map  [char *] map two letter code
** @param [r] mE   [double] map easting (metres)
** @param [r] mN   [double] map northing (metres)
** @param [w] lat  [double *] WGS84 latitude (deg)
** @param [w] lon  [double *] WGS84 longitude (deg)
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UKOSMap_To_WGS84_C(const char* map, double mE, double mN,
                                    double* lat, double* lon)
{
  double E;
  double N;
  double alat;
  double alon;
  double ht;

  if (!GPS_Math_UKOSNG_Map_To_EN(map,mE,mN,&E,&N)) {
    return 0;
  }

  GPS_Math_NGENToAiry1830LatLon(E,N,&alat,&alon);

  GPS_Math_Known_Datum_To_WGS84_C(alat,alon,0,lat,lon,&ht,86);

  return 1;
}


/* @funcstatic GPS_Math_LatLon_To_UTM_Param *****************************
**
** Transform NAD33
**
** @param [r] lat  [double] NAD latitude (deg)
** @param [r] lon  [double] NAD longitude (deg)
** @param [w] zone [int32 *]  zone number
** @param [w] zc   [char *] zone character
** @param [w] Mc   [double *] central meridian
** @param [w] E0   [double *] false easting
** @param [w] N0   [double *] false northing
** @param [w] F0   [double *] scale factor
**
** @return [int32] success
************************************************************************/
static int32_t GPS_Math_LatLon_To_UTM_Param(double lat, double lon, int32_t* zone,
                                            char* zc, double* Mc, double* E0,
                                            double* N0, double* F0)
{
	int32_t ilon;
	int32_t ilat;
  bool psign;
  bool lsign;

  if (lat >= 84.0 || lat < -80.0) {
    return 0;
  }

  psign = lsign = false;
  if (lon < 0.0) {
    lsign=true;
  }
  if (lat < 0.0) {
    psign=true;
  }

  ilon = abs((int32_t)lon);
  ilat = abs((int32_t)lat);

  if (!lsign) {
    *zone = 31 + (ilon / 6);
    *Mc   = static_cast<double>((ilon / 6) * 6 + 3);
  } else {
    *zone = 30 - (ilon / 6);
    *Mc   = -static_cast<double>((ilon / 6) * 6 + 3);
  }

  if (!psign) {
    *zc = 'N' + ilat / 8;
    if (*zc > 'N') {
      ++*zc;
    }
  } else {
    *zc = 'M' - (ilat / 8);
    if (*zc <= 'I') {
      --*zc;
    }
  }

  // The northernmost latitude band, X, is 12 degrees tall
  // instead of the usual 8 degrees, i.e. [72.0,84.0).
  if (*zc=='Y') {
    *zc = 'X';
  }

  // Norway exception
  if (*zc=='V' && lon>=3.0 && lon<12.0) {
    *zone = 32;
    *Mc   = 9.0;
  }

  // Svalbard exception
  if (*zc=='X' && lon>=0.0 && lon<42.0) {
    if (lon<9.0) {
      *zone = 31;
      *Mc   = 3.0;
    } else if (lon<21.0) {
      *zone = 33;
      *Mc   = 15.0;
    } else if (lon<33.0) {
      *zone = 35;
      *Mc   = 27.0;
    } else {
      *zone = 37;
      *Mc   = 39.0;
    }
  }

  if (!psign) {
    *N0 = 0.0;
  } else {
    *N0 = 10000000.0;
  }

  *E0 = 500000.0;
  *F0 = 0.9996;

  return 1;
}



/* @func GPS_Math_NAD83_To_UTM_EN **************************************
**
** Transform NAD33 lat/lon to UTM zone, easting and northing
**
** @param [r] lat  [double] NAD latitude (deg)
** @param [r] lon  [double] NAD longitude (deg)
** @param [w] E    [double *] easting (metres)
** @param [w] N    [double *] northing (metres)
** @param [w] zone [int32 *]  zone number
** @param [w] zc   [char *] zone character
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_NAD83_To_UTM_EN(double lat, double lon, double* E,
                                 double* N, int32_t* zone, char* zc)
{
  double phi0;
  double lambda0;
  double N0;
  double E0;
  double F0;

  if (!GPS_Math_LatLon_To_UTM_Param(lat,lon,zone,zc,&lambda0,&E0,
                                    &N0,&F0)) {
    return 0;
  }

  phi0 = 0.0;

  constexpr double a = GRS80_Ellipse.a;
  constexpr double b = GRS80_Ellipse.b();

  GPS_Math_LatLon_To_EN(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b);

  return 1;
}



/* @func GPS_Math_WGS84_To_UTM_EN **************************************
**
** Transform WGS84 lat/lon to UTM zone, easting and northing
**
** @param [r] lat  [double] WGS84 latitude (deg)
** @param [r] lon  [double] WGS84 longitude (deg)
** @param [w] E    [double *] easting (metres)
** @param [w] N    [double *] northing (metres)
** @param [w] zone [int32 *]  zone number
** @param [w] zc   [char *] zone character
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_WGS84_To_UTM_EN(double lat, double lon, double* E,
                                 double* N, int32_t* zone, char* zc)
{
  double phi;
  double lambda;
  double H;

  GPS_Math_WGS84_To_Known_Datum_M(lat,lon,0,&phi,&lambda,&H,77);
  if (!GPS_Math_NAD83_To_UTM_EN(phi,lambda,E,N,zone,zc)) {
    return 0;
  }

  return 1;
}



/* @funcstatic GPS_Math_UTM_Param_To_Mc ********************************
**
** Convert UTM zone and zone character to central meridian value.
** Also return false eastings, northings and scale factor
**
** @param [w] zone [int32]  zone number
** @param [w] zc   [char] zone character
** @param [w] Mc   [double *] central meridian
** @param [w] E0   [double *] false easting
** @param [w] N0   [double *] false northing
** @param [w] F0   [double *] scale factor
**
** @return [int32] success
************************************************************************/
static int32_t GPS_Math_UTM_Param_To_Mc(int32_t zone, char zc, double* Mc,
                                        double* E0, double* N0, double* F0)
{

  if (zone>60 || zone<0 || zc<'C' || zc>'X' || zc=='I' || zc=='O') {
    return 0;
  }

  *Mc = static_cast<double>(((zone-31)*6) + 3);

  // The Norway and Svalbard exceptions do NOT change the central meridian of
  // the impacted zones (31V, 32V, 31X, 33X, 35X, 37X).

  if (zc>'M') {
    *N0 = 0.0;
  } else {
    *N0 = 10000000.0;
  }

  *E0 = 500000.0;
  *F0 = 0.9996;

  return 1;
}



/* @func GPS_Math_UTM_EN_To_NAD83 **************************************
**
** Transform UTM zone, easting and northing to NAD83 lat/lon
**
** @param [r] lat  [double *] NAD latitude (deg)
** @param [r] lon  [double *] NAD longitude (deg)
** @param [w] E    [double] easting (metres)
** @param [w] N    [double] northing (metres)
** @param [w] zone [int32]    zone number
** @param [w] zc   [char]   zone character
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UTM_EN_To_NAD83(double* lat, double* lon, double E,
                                 double N, int32_t zone, char zc)
{
  return GPS_Math_UTM_EN_To_Known_Datum(lat, lon, E, N, zone, zc, 77);
}



/* @func GPS_Math_UTM_EN_To_WGS84 **************************************
**
** Transform UTM zone, easting and northing to WGS84 lat/lon
**
** @param [w] lat  [double *] WGS84 latitude (deg)
** @param [r] lon  [double *] WGS84 longitude (deg)
** @param [w] E    [double]   easting (metres)
** @param [w] N    [double]   northing (metres)
** @param [w] zone [int32]      zone number
** @param [w] zc   [char]     zone character
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UTM_EN_To_WGS84(double* lat, double* lon, double E,
                                 double N, int32_t zone, char zc)
{
  double lambda0;
  double N0;
  double E0;
  double F0;

  if (!GPS_Math_UTM_Param_To_Mc(zone,zc,&lambda0,&E0,&N0,&F0)) {
    return 0;
  }

  GPS_Math_UTM_EN_to_LatLon(GPS_Datums[118].ellipse, N, E, lat, lon, lambda0, E0, N0);
  return 1;
}


/* @func GPS_Math_Known_Datum_To_UTM_EN *********************************
**
** Transform known datum lat/lon to UTM zone, easting and northing
**
** @param [r] lat  [double] WGS84 latitude (deg)
** @param [r] lon  [double] WGS84 longitude (deg)
** @param [w] E    [double *] easting (metres)
** @param [w] N    [double *] northing (metres)
** @param [w] zone [int32 *]  zone number
** @param [w] zc   [char *] zone character
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_Known_Datum_To_UTM_EN(double lat, double lon, double* E,
                                       double* N, int32_t* zone, char* zc, const int n)
{
  double phi0;
  double lambda0;
  double N0;
  double E0;
  double F0;
  double a;
  double b;
  int32_t idx;

  if (!GPS_Math_LatLon_To_UTM_Param(lat,lon,zone,zc,&lambda0,&E0,
                                    &N0,&F0)) {
    return 0;
  }

  phi0 = 0.0;

  idx  = GPS_Datums[n].ellipse;
  a = GPS_Ellipses[idx].a;
  b = GPS_Ellipses[idx].b();

  GPS_Math_LatLon_To_EN(E,N,lat,lon,N0,E0,phi0,lambda0,F0,a,b);

  return 1;
}

/* @func GPS_Math_UTM_EN_To_Known_Datum *********************************
**
** Transform UTM zone, easting and northing to known datum lat/lon
**
** @param [w] lat  [double *] WGS84 latitude (deg)
** @param [r] lon  [double *] WGS84 longitude (deg)
** @param [w] E    [double]   easting (metres)
** @param [w] N    [double]   northing (metres)
** @param [w] zone [int32]      zone number
** @param [w] zc   [char]     zone character
** @param [r] n    [int32] datum number from GPS_Datums structure
**
** @return [int32] success
************************************************************************/
int32_t GPS_Math_UTM_EN_To_Known_Datum(double* lat, double* lon, double E,
                                       double N, int32_t zone, char zc, const int n)
{
  double lambda0;
  double N0;
  double E0;
  double F0;

  if (!GPS_Math_UTM_Param_To_Mc(zone,zc,&lambda0,&E0,&N0,&F0)) {
    return 0;
  }

  GPS_Math_UTM_EN_to_LatLon(GPS_Datums[n].ellipse, N, E, lat, lon, lambda0, E0, N0);
  return 1;
}

/* !!! copied from unused gpsproj.c !!! */

/* @func GPS_Math_Swiss_LatLon_To_EN ***********************************
**
** Convert latitude and longitude to Swiss grid easting and northing
**
** @param [r] phi [double] latitude (deg)
** @param [r] lambda [double] longitude (deg)
** @param [w] E [double *] easting (metre)
** @param [w] N [double *] northing (metre)
** @param [r] phi0 [double] latitude origin (deg)     [normally 46.95240556]
** @param [r] lambda0 [double] longitude origin (deg) [normally  7.43958333]
** @param [r] E0 [double] false easting (metre)       [normally 600000.0]
** @param [r] N0 [double] false northing (metre)      [normally 200000.0]
** @param [r] a [double] semi-major axis              [normally 6377397.000]
** @param [r] b [double] semi-minor axis              [normally 6356078.823]
**
** @return [void]
***************************************************************************/
void GPS_Math_Swiss_LatLon_To_EN(double phi, double lambda, double* E,
                                 double* N,double phi0,double lambda0,
                                 double E0, double N0, double a, double b)

{
  double a2;
  double b2;
  double esq;
  double e;
  double c;
  double ephi0p;
  double phip;
  double sphip;
  double phid;
  double slambda2;
  double lambda1;
  double lambda2;
  double K;
  double po4;
  double w;
  double R;

  lambda0 = GPS_Math_Deg_To_Rad(lambda0);
  phi0    = GPS_Math_Deg_To_Rad(phi0);
  lambda  = GPS_Math_Deg_To_Rad(lambda);
  phi     = GPS_Math_Deg_To_Rad(phi);

  po4=GPS_PI/4.0;

  a2 = a*a;
  b2 = b*b;
  esq = (a2-b2)/a2;
  e   = pow(esq,0.5);

  c = sqrt(1+((esq*pow(cos(phi0),4.))/(1.-esq)));

  ephi0p = asin(sin(phi0)/c);

  K = log(tan(po4+ephi0p/2.)) - c*(log(tan(po4+phi0/2.)) -
      e/2. * log((1.+e*sin(phi0)) /
                         (1.-e*sin(phi0))));
  lambda1 = c*(lambda-lambda0);
  w = c*(log(tan(po4+phi/2.)) - e/2. *
         log((1.+e*sin(phi)) / (1.-e*sin(phi)))) + K;


  phip = 2. * (atan(exp(w)) - po4);

  sphip = cos(ephi0p) * sin(phip) - sin(ephi0p) * cos(phip) * cos(lambda1);
  phid  = asin(sphip);

  slambda2 = cos(phip)*sin(lambda1) / cos(phid);
  lambda2  = asin(slambda2);

  R = a*sqrt(1.-esq) / (1.-esq*sin(phi0) * sin(phi0));

  *N = R*log(tan(po4 + phid/2.)) + N0;
  *E = R*lambda2 + E0;
  return;
}

/* !!! copied from unused gpsproj.c !!! */

/* @func GPS_Math_Swiss_EN_To_LatLon ************************************
**
** Convert Swiss Grid easting and northing to latitude and longitude
**
** @param [r] E [double] easting (metre)
** @param [r] N [double] northing (metre)
** @param [w] phi [double *] latitude (deg)
** @param [w] lambda [double *] longitude (deg)
** @param [r] phi0 [double] latitude origin (deg)     [normally 46.95240556]
** @param [r] lambda0 [double] longitude origin (deg) [normally  7.43958333]
** @param [r] E0 [double] false easting (metre)       [normally 600000.0]
** @param [r] N0 [double] false northing (metre)      [normally 200000.0]
** @param [r] a [double] semi-major axis              [normally 6377397.000]
** @param [r] b [double] semi-minor axis              [normally 6356078.823]
**
** @return [void]
*************************************************************************/

void GPS_Math_Swiss_EN_To_LatLon(double E, double N, double* phi,
                                 double* lambda, double phi0, double lambda0,
                                 double E0, double N0, double a, double b)
{
  double a2;
  double b2;
  double esq;
  double e;
  double R;
  double c;
  double po4;
  double phid;
  double phi1;
  double lambdad;
  double lambda1;
  double slambda1;
  double ephi0p;
  double sphip;
  double tol;
  double cr;
  double C;
  double K;

  lambda0 = GPS_Math_Deg_To_Rad(lambda0);
  phi0    = GPS_Math_Deg_To_Rad(phi0);

  po4=GPS_PI/4.0;
  tol=0.00001;

  a2 = a*a;
  b2 = b*b;
  esq = (a2-b2)/a2;
  e   = pow(esq,0.5);

  R = a*sqrt(1.-esq) / (1.-esq*sin(phi0) * sin(phi0));

  phid = 2.*(atan(exp((N - N0)/R)) - po4);
  lambdad = (E - E0)/R;

  c = sqrt(1.+((esq * pow(cos(phi0), 4.)) /
                       (1.-esq)));
  ephi0p = asin(sin(phi0) / c);

  sphip = cos(ephi0p)*sin(phid) + sin(ephi0p)*cos(phid)*cos(lambdad);
  phi1 = asin(sphip);

  slambda1 = cos(phid)*sin(lambdad)/cos(phi1);
  lambda1  = asin(slambda1);

  *lambda = GPS_Math_Rad_To_Deg((lambda1/c + lambda0));

  K = log(tan(po4 + ephi0p/2.)) -c*(log(tan(po4 + phi0/2.))
      - e/2. * log((1.+e*sin(phi0)) /
                           (1.-e*sin(phi0))));
  C = (K - log(tan(po4 + phi1/2.)))/c;

  do {
    cr = (C + log(tan(po4 + phi1/2.)) - e/2. *
          log((1.+e*sin(phi1)) / (1.-e*sin(phi1)))) *
         (((1.-esq*sin(phi1)*sin(phi1)) * cos(phi1)) /
          (1.-esq));
    phi1 -= cr;
  } while (fabs(cr) > tol);

  *phi = GPS_Math_Rad_To_Deg(phi1);

  return;
}

/********************************************************************/

void GPS_Math_UTM_EN_to_LatLon(int ReferenceEllipsoid,
                               const double UTMNorthing, const double UTMEasting,
                               double* Lat, double* Lon,
                               const double lambda0,
                               const double E0,
                               const double N0)
{
//converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
//East Longitudes are positive, West longitudes are negative.
//North latitudes are positive, South latitudes are negative
//Lat and Long are in decimal degrees.
//based on code written by Chuck Gantz- chuck.gantz@globalstar.com
//found at http://www.gpsy.com/gpsinfo/geotoutm/index.html

  constexpr double k0 = 0.9996;
  double a, f;
  double eccSquared;
  double eccPrimeSquared;
  double e1;
  double N1, T1, C1, R1, D, M;
  double mu, phi1Rad;
  double x, y;

  a = GPS_Ellipses[ReferenceEllipsoid].a;
  f = 1 / GPS_Ellipses[ReferenceEllipsoid].invf;
  eccSquared = f * (2.0 - f);
  e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));

  x = UTMEasting - E0; //remove false easting
  y = UTMNorthing - N0; //remove false northing

  eccPrimeSquared = (eccSquared)/(1-eccSquared);

  M = y / k0;
  mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));

  phi1Rad = mu+ (3*e1/2-27*e1*e1*e1/32)*sin(2*mu) +
            (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu) +
            (151*e1*e1*e1/96)*sin(6*mu);

  N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
  T1 = tan(phi1Rad)*tan(phi1Rad);
  C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
  R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
  D = x/(N1*k0);

  *Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
                                         +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
  *Lat = GPS_Math_Rad_To_Deg(*Lat);

  *Lon = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)*D*D*D*D*D/120)/cos(phi1Rad);
  *Lon = lambda0 + GPS_Math_Rad_To_Deg(*Lon);
}

/********************************************************************/

int32_t GPS_Lookup_Datum_Index(const char* n)
{
  const GPS_Datum* dp;
  const GPS_Datum_Alias* al;

  for (al = GPS_DatumAliases; al->alias; al++) {
    if (case_ignore_strcmp(al->alias, n) == 0) {
      return al->datum;
    }
  }

  for (dp = GPS_Datums; dp->name; dp++) {
    if (0 == case_ignore_strcmp(dp->name, n)) {
      return dp - GPS_Datums;
    }
  }

  return -1;
}

int32_t GPS_Lookup_Datum_Index(const QString& n)
{
  return GPS_Lookup_Datum_Index(CSTR(n));
}

const char*
GPS_Math_Get_Datum_Name(const int datum_index)
{
  return GPS_Datums[datum_index].name;
}


