/* EarthDist - calculate distance on earth surface
 * Copyright (C) 2004 Tim Janik
 *
 * This software is provided "as is"; redistribution and modification
 * is permitted, provided that the following disclaimer is retained.
 *
 * This software 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.
 * In no event shall the authors or contributors be liable for any
 * direct, indirect, incidental, special, exemplary, or consequential
 * damages (including, but not limited to, procurement of substitute
 * goods or services; loss of use, data, or profits; or business
 * interruption) however caused and on any theory of liability, whether
 * in contract, strict liability, or tort (including negligence or
 * otherwise) arising in any way out of the use of this software, even
 * if advised of the possibility of such damage.
 *
 * This code was transcribed from fortran into C by Tim Janik. the original
 * fortran code, which is based on the April, 1975 paper published in Survey
 * Review by T. Vincenty, can be obtained from
 *      http://www.ngs.noaa.gov/PC_PROD/Inv_Fwd/.
 */
#include "earthdist.h"
#include <math.h>
#include <stdio.h>

#if 1   /* WGS84 */
#define A       (6378137.0)             /* semi-major axis of reference ellipsoid (in meters) */
#define F       (1.0 / 298.25722356300) /* flattening */
#else   /* GRS80 */
#define A       (6378137.0)             /* semi-major axis of reference ellipsoid (in meters) */
#define F       (1.0 / 298.25722210088) /* flattening */
#endif
#define ESQ     (F * (2.0 - F))         /* eccentricity squared for reference ellipsoid */
#define TWOPI   (M_PI + M_PI)
#define SQR(v)  ((v) * (v))
#ifndef FALSE
#define FALSE   (0)
#endif
#ifndef TRUE
#define TRUE    (!FALSE)
#endif

/* name:        gpnarc
 * version:     200005.26
 * written by:  Robert (Sid) Safford
 * purpose:     subroutine to compute the length of a meridional arc
 *              between two latitudes
 *
 * comments:
 *********1*********2*********3*********4*********5*********6*********7*
 * ::modification history
 * ::197507.05, rws, ver 00 tencol released for field use
 * ::198311.20, rws, ver 01 mten   released to field
 * ::198411.26, rws, ver 07 mten2  released to field
 * ::1985xx.xx, rws, code   created
 * ::198506.10, rws, wrk    enhancements released to field 
 * ::198509.01, rws, ver 11 mten3  released to field 
 * ::198512.18, rws, code   modified for mten3 
 * ::198708.10, rws, code   modified to use new mten4 gpn record format 
 * ::199112.31, rws, ver 20 mten4 released to field 
 * ::200001.13, rws, ver 21 mten4 released to field 
 * ::200005.26, rws, code   restructured & documentation added 
 * ::200012.31, rws, ver 23 mten5 released 
 * ********1*********2*********3*********4*********5*********6*********7* 
 * e::gpnarc 
 * --------------------------- 
 *     m t e n  (version 3) 
 *     m t e n  (version 5.23) 
 * ---------------------------
 */
static int
gpnarc (double  amax,   /* semi-major axis of reference ellipsoid */
        double  p1,     /* lat station 1 (in radians) */
        double  p2,     /* lat station 2 (in radians) */
        double *arc)    /* geodetic distance */
{
  static const double tt = 5e-15;
  double a, b, c, d, e, f, e2, e4, e6, e8, s1, s2, t1, t2, t3, t4, t5, da, db, dc, dd, de, df, ex;
  int flag = FALSE;

  /* check for a 90 degree lookup */
  s1 = fabs (p1);
  s2 = fabs (p2);

  if (M_PI_2 - tt < s2 && s2 < M_PI_2 + tt)
    flag = TRUE;

  if (s1 > tt)
    flag = FALSE;

  da = p2 - p1;
  s1 = 0.0;
  s2 = 0.0;

  /* compute the length of a meridional arc between two latitudes */
  e2 = ESQ;
  e4 = e2 * e2;
  e6 = e4 * e2;
  e8 = e6 * e2;
  ex = e8 * e2;
  
  t1 = e2 * 0.75;
  t2 = e4 * 0.234375;
  t3 = e6 * 0.068359375;
  t4 = e8 * 0.01922607421875;
  t5 = ex * 0.00528717041015625;

  a = t1 + 1.0 + t2 * 3.0 + t3 * 10.0 + t4 * 35.0 + t5 * 126.0;

  if (flag)
    goto L1;

  b = t1 + t2 * 4.0 + t3 * 15.0 + t4 * 56.0 + t5 * 210.0;
  c = t2 + t3 * 6.0 + t4 * 28.0 + t5 * 120.0;
  d = t3 + t4 * 8.0 + t5 * 45.0;
  e = t4 + t5 * 10.0;
  f = t5;
  
  db = sin (p2 * 2.0) - sin (p1 * 2.0);
  dc = sin (p2 * 4.0) - sin (p1 * 4.0);
  dd = sin (p2 * 6.0) - sin (p1 * 6.0);
  de = sin (p2 * 8.0) - sin (p1 * 8.0);
  df = sin (p2 * 10.0) - sin (p1 * 10.0);
  
  /* compute the s2 part of the series expansion */
  s2 = -db * b / 2.0 + dc * c / 4.0 - dd * d / 6.0 + de * e / 8.0 - df * f / 10.0;

 L1:
  /* compute the s1 part of the series expansion */
  s1 = da * a;

  /* compute the arc length */
  *arc = amax * (1.0 - ESQ) * (s1 + s2);

  return 0;
}       /* gpnarc() */

/* name:        gpnloa
 * version:     200005.26
 * written by:  Robert (Sid) Safford
 * purpose:     subroutine to compute the liff-off-azimuth constants
 *
 * comments:
 *********1*********2*********3*********4*********5*********6*********7*
 * ::modification history
 * ::1985xx.xx, rws, code   created
 * ::198506.10, rws, wrk    enhancements released to field
 * ::198509.01, rws, ver 11 mten3  released to field
 * ::198512.18, rws, code   modified for mten3
 * ::198708.10, rws, code   modified to use new mten4 gpn record format
 * ::199112.31, rws, ver 20 mten4 released to field
 * ::200001.13, rws, ver 21 mten4 released to field
 * ::200005.26, rws, code   restructured & documentation added
 * ::200012.31, rws, ver 23 mten5 released
 *********1*********2*********3*********4*********5*********6*********7*
 * e::gpnloa
 * ---------------------------
 *     m t e n  (version 3)
 *              (version 4.22)
 *              (version 5.23)
 * ---------------------------
 */
static int
gpnloa (double  amax,   /* semi-major axis of reference ellipsoid */
        double *dl,     /* lon difference */
        double *az1,    /* azi at sta 1 -> sta 2 */
        double *az2,    /* az2 at sta 2 -> sta 1 */
        double *ao,     /* const */
        double *bo,     /* const */
        double *sms)    /* distance ... equatorial - geodesic  (S - s)   "SMS" */
{
  static const double tt = 5e-13;
  double f, s, c2, t1, t2, u2, t4, u4, t6, u6, t8, u8, cs, az, dlon, cons;
  int iter;
  double esqp;

  dlon = fabs (*dl);
  cons = (M_PI - dlon) / (M_PI * F);
  f = F;

  /* compute an approximate az */
  az = asin (cons);
  t1 = 1.0;
  t2 = f * -0.25 * (f + 1.0 + f * f);
  t4 = f * 0.1875 * f * (f * 2.25 + 1.0);
  t6 = f * -0.1953125 * f * f;

  iter = 0;
 L1:
  ++iter;
  s = cos (az);
  c2 = s * s;

  /* compute new ao */
  *ao = t1 + t2 * c2 + t4 * c2 * c2 + t6 * c2 * c2 * c2;
  cs = cons / *ao;
  s = asin (cs);
  if (fabs (s - az) < tt)
    goto L2;

  az = s;
  if (iter <= 6)
    goto L1;

 L2:
  *az1 = s;
  if (*dl < 0.0)
    *az1 = TWOPI - *az1;
  *az2 = TWOPI - *az1;
  
  /* equatorial - geodesic  (S - s)   "SMS" */
  esqp = ESQ / (1.0 - ESQ);
  s = cos (*az1);

  u2 = esqp * s * s;
  u4 = u2 * u2;
  u6 = u4 * u2;
  u8 = u6 * u2;
  
  t1 = 1.0;
  t2 = u2 * 0.25;
  t4 = u4 * -0.046875;
  t6 = u6 * 0.01953125;
  t8 = u8 * -0.01068115234375;

  *bo = t1 + t2 + t4 + t6 + t8;
  s = sin (*az1);
  *sms = amax * M_PI * (1. - F * fabs (s) * *ao - *bo * (1.0 - F));

  return 0;
}       /* gpnloa() */

/* name:        gpnhri
 * version:     200208.09
 * written by:  robert (sid) safford
 * purpose:     subroutine to compute helmert rainsford inverse problem
 *
 * solution of the geodetic inverse problem after t. vincenty
 * modified rainsford's method with helmert's elliptical terms
 * effective in any azimuth and at any distance short of antipocal
 * from/to stations must not be the geographic pole.
 * parameter a is the semi-major axis of the reference ellipsoid
 * finv=1/f is the inverse flattening of the reference ellipsoid
 * latitudes and longitudes in radians positive north and west
 * forward and back azimuths returned in radians clockwise from south
 * geodesic distance s returned in units of semi-major axis a
 * programmed for ibm 360-195   09/23/75
 *
 * note - note - note -
 * 1. do not use for meridional arcs and be careful on the equator.
 * 2. azimuths are from north(+) clockwise and
 * 3. longitudes are positive east(+)
 *
 * comments:
 *********1*********2*********3*********4*********5*********6*********7*
 * ::modification history
 * ::197507.05, rws, ver 00 tencol released for field use
 * ::198311.20, rws, ver 01 mten   released to field
 * ::198411.26, rws, ver 07 mten2  released to field
 * ::198506.10, rws, wrk    enhancements released to field
 * ::198507.22, rws, code   modified for mten3
 * ::198509.01, rws, ver 11 mten3  released to field
 * ::198708.10, rws, code   modified to use new mten4 gpn record format
 * ::199112.31, rws, ver 20 mten4 released to field
 * ::200001.13, rws, ver 21 mten4 released to field
 * ::200005.26, rws, code   restructured & documentation added
 * ::200012.31, rws, ver 23 mten5 released
 * ::200104.09, rws, code   added to calblin program
 * ::200208.09, rws, code   added subroutines gpnarc & gpnloa
 *********1*********2*********3*********4*********5*********6*********7*
 * e::gpnhri
 *  -------------------------------
 *     m t e n  (version 3)
 *              (version 4.22)
 *              (version 5.23)
 *  -------------------------------
 */
static int
gpnhri (double  p1,     /* lat station 1 (in radians) */
        double  e1,     /* lon station 1 (in radians) */
        double  p2,     /* lat station 2 (in radians) */
        double  e2,     /* lon station 2 (in radians) */
        double *az1,    /* azimuth forward at station 1 -> station 2 (in radians) */
        double *az2,    /* azimuth back at station 2 -> station 1 (in radians) */
        double *s)      /* geodetic distance between stations 1 & 2 (in meters) */
{
  static const double tol0 = 5e-15;     /* tolerance for checking computation value */
  static const double tol1 = 5e-14;     /* tolerance for checking a real zero value */
  static const double tol2 = 0.007;     /* tolerance for close to zero value */
  double aa;            /* constant from subroutine gpnloa */
  double bb;            /* constant from subroutine gpnloa */
  double alimit;        /* equatorial arc distance along the equator (radians) */
  double arc;           /* meridional arc distance latitude p1 to p2 (in meters) */
  double dlon;          /* temporary value for difference in longitude (radians) */
  double equ;           /* equatorial distance (in meters) */
  double r1, r2, ss;    /* temporary variables */
  double sms;           /* equatorial - geodesic distance (S - s) "Sms" */

  /* Local variables */
  double b, w, z, a2, b2, a4, f0, f2, f3, f4, a6, b4, b6, q2;
  double u1, u2, t4, q4, t6, q6, r3, ab, ao, bo, qo, xy, xz;
  double cu1, cu2, su1, su2, sig, csig, clon, ssig;
  double epsq, slon;
  double sinalf;

  int kount;

  /* test the longitude difference with tol1
   * tol1 is approximately 0.000000001 arc seconds
   */
  ss = e2 - e1;
  if (fabs (ss) < tol1)
    {
      fprintf (stderr, "longitudal difference is near zero\n");
      e2 += tol1;
      r2 = p2;
      r1 = p1;
      gpnarc (M_PI, r1, r2, &arc);
      *s = fabs (arc);

      if (p2 > p1)
        {
          *az1 = 0.0;
          *az2 = M_PI;
        }
      else
        {
          *az1 = M_PI;
          *az2 = 0.0;
        }
      return 0;
    }

  /* test for longitude over 180 degrees */
  dlon = e2 - e1;
  if (dlon >= 0.)
    {
      if (M_PI <= dlon && dlon < TWOPI)
        dlon -= TWOPI;
    }
  else
    {
      ss = fabs (dlon);
      if (M_PI <= ss && ss < TWOPI)
        dlon += TWOPI;
    }

  ss = fabs (dlon);
  if (ss > M_PI)
    {
      /* fprintf (stderr, "Longitude difference over 180 degrees, Turn it around\n"); */
      ss = TWOPI - ss;
    }

  /* compute the limit in longitude (alimit), it is equal
   * to twice the distance from the equator to the pole,
   * as measured along the equator (east/ewst)
   */
  alimit = M_PI * (1.0 - F);

  /* test for anti-nodal difference */
  if (ss >= alimit)
    {
      r1 = fabs (p1);
      r2 = fabs (p2);
      /* latitudes r1 & r2 are not near the equator */
      if (r1 > tol2 && r2 > tol2)
        goto L60;

      /* longitude difference is greater than lift-off point
       * now check to see if "both"  r1 & r2 are on equator
       */
      if (r1 < tol1 && r2 > tol2)
        goto L60;
      if (r2 < tol1 && r1 > tol2)
        goto L60;

      /* check for either r1 or r2 just off the equator but < tol2 */
      if (r1 > tol1 || r2 > tol1)
        {
          *az1 = 0.;
          *az2 = 0.;
          *s = 0.;
          return 0;
	}

      /* compute the azimuth to anti-nodal point */

      /* fprintf (stderr, "Longitude difference beyond lift-off point\n"); */

      gpnloa (M_PI, &dlon, az1, az2, &aa, &bb, &sms);

      /* compute the equatorial distance & geodetic */
      equ = A * fabs (dlon);
      *s = equ - sms;
      return 0;
    }

 L60:
  f0 = 1.0 - F;
  b = A * f0;
  epsq = ESQ / (1.0 - ESQ);
  f2 = F * F;
  f3 = F * f2;
  f4 = F * f3;

  /* the longitude difference */
  dlon = e2 - e1;
  ab = dlon;
  kount = 0;

  /* the reduced latitudes */
  u1 = f0 * sin (p1) / cos (p1);
  u2 = f0 * sin (p2) / cos (p2);
  
  u1 = atan (u1);
  u2 = atan (u2);
  
  su1 = sin (u1);
  cu1 = cos (u1);
  
  su2 = sin (u2);
  cu2 = cos (u2);
  
  /* counter for the iteration operation */
 L1:
  ++kount;
  
  clon = cos (ab);
  slon = sin (ab);

  csig = su1 * su2 + cu1 * cu2 * clon;
  ssig = sqrt (SQR (slon * cu2) + SQR (su2 * cu1 - su1 * cu2 * clon));

  sig = atan2 (ssig, csig);
  sinalf = cu1 * cu2 * slon / ssig;

  w = 1.0 - sinalf * sinalf;
  t4 = w * w;
  t6 = w * t4;

  /* the coefficients of type a */
  ao = F - f2 * (F + 1.0 + f2) * w / 4.0 + f3 * 3.0 * (F * 9.0 / 4.0 + 1.0) * 
       t4 / 16.0 - f4 * 25.0 * t6 / 128.0;
  a2 = f2 * (F + 1.0 + f2) * w / 4.0 - f3 * (F * 9.0 / 4.0 + 1.0) * t4 / 4.0 + 
       f4 * 75.0 * t6 / 256.0;
  a4 = f3 * (F * 9.0 / 4.0 + 1.0) * t4 / 32.0 - f4 * 15.0 * t6 / 256.0;
  a6 = f4 * 5.0 * t6 / 768.0;

  /* the multiple angle functions */
  qo = 0.0;
  if (w > tol0)
    qo = su1 * -2.0 * su2 / w;

  q2 = csig + qo;
  q4 = q2 * 2.0 * q2 - 1.0;
  q6 = q2 * (q2 * 4.0 * q2 - 3.0);
  r2 = ssig * 2.0 * csig;
  r3 = ssig * (3.0 - ssig * 4.0 * ssig);

  /* the longitude difference */
  *s = sinalf * (ao * sig + a2 * ssig * q2 + a4 * r2 * q4 + a6 * r3 * q6);
  xz = dlon + *s;

  // xy  = dabs(xz-ab)
  xy = fabs (xz - ab);
  ab = dlon + *s;

  if (xy < 5e-14)
    goto L4;
  
  if (kount <= 7)
    goto L1;
  
  /* the coefficients of type b */
  
 L4:
  z = epsq * w;
  bo = z * (z * (z * (0.01953125 - z * 175.0 / 16384.0) - 0.046875) + 0.25) + 1.0;
  b2 = z * (z * (z * (z * 35.0 / 2048.0 - 0.029296875) + 0.0625) - 0.25);
  b4 = z * z * (z * (0.005859375 - z * 35.0 / 8192.0) - 0.0078125);
  b6 = z * z * z * (z * 5.0 / 6144.0 - 6.5104166666666663e-4);

  /* the distance in meters */
  *s = b * (bo * sig + b2 * ssig * q2 + b4 * r2 * q4 + b6 * r3 * q6);

  /* first compute the az1 & az2 for along the equator */
  if (dlon > M_PI)
    dlon -= TWOPI;
  if (fabs (dlon) > M_PI)
    dlon += TWOPI;

  *az1 = M_PI_2;
  if (dlon < 0.0)
    *az1 *= 3.0;

  *az2 = *az1 + M_PI;
  if (*az2 > TWOPI)
    *az2 -= TWOPI;

  /* now compute the az1 & az2 for latitudes not on the equator */
  if (!(fabs (su1) < tol0 && fabs (su2) < tol0))
    {
      double tana1 = slon * cu2 / (su2 * cu1 - clon * su1 * cu2);
      double tana2 = slon * cu1 / (su1 * cu2 - clon * su2 * cu1);
      double sina1 = sinalf / cu1;
      double sina2 = -sinalf / cu2;

      /* azimuths from north, longitudes positive east */
      *az1 = atan2 (sina1, sina1 / tana1);
      *az2 = M_PI - atan2 (sina2, sina2 / tana2);
    }

  if (*az1 < 0.0)
    *az1 += TWOPI;

  if (*az2 < 0.0)
    *az2 += TWOPI;

  return 0;
}       /* gpnhri() */

double
gps_earth_distance (double  latitude1,
                    double  longitude1,
                    double  latitude2,
                    double  longitude2,
                    double *azimuth_forw,
                    double *azimuth_back)
{
  double dummy1, dummy2, geodetic_distance = 0;

  gpnhri (latitude1 * M_PI / 180.0,
          longitude1 * M_PI / 180.0,
          latitude2 * M_PI / 180.0,
          longitude2 * M_PI / 180.0,
          azimuth_forw ? azimuth_forw : &dummy1,
          azimuth_back ? azimuth_back : &dummy2,
          &geodetic_distance);

  return geodetic_distance;
}

double
gps_earth_latitude (double   degree,
                    double   minute,
                    double   seconds,
                    int      is_south)
{
  double latlong = degree + minute * (1. / 60.) + seconds * (1. / 3600.);
  return is_south ? -latlong : latlong;
}

double
gps_earth_longitude (double   degree,
                     double   minute,
                     double   seconds,
                     int      is_west)
{
  double latlong = degree + minute * (1. / 60.) + seconds * (1. / 3600.);
  return is_west ? -latlong : latlong;
}