/*
 * jordist.c
 *
 * Compute distance/direction from lat/long pairs based on Jordan's
 * formula, as published in "The Radio Amateur's World Atlas"
 * by Folke Rosvall SM5AGM.
 *
 * This formula is *not* accurate for very small distances.  If so,
 * a recomputation based on a planarization is recommended.
 *
 */

#include <math.h>

static double lfunc(double *kv)
{
  double k = *kv;

  if ( k >= 1.0 ) { *kv = 1.0;  return 0.0; }
  if ( k <= -1.0 ) { *kv = -1.0; return M_PI; }
  return M_PI_2 - atan(k/sqrt(1.0-k*k));
}

/*
 * Distance is returned in km.
 */

void jordist(double lat1, double long1, double lat2, double long2,
	     double *distance, double *heading)
{
  static const double x0 = 6378.140;    /* Equatorial radius of the Earth */
  static const double x1 = 6356.755;    /* Polar radius of the Earth */
  static const double x2 = 0.014;       /* ??? */
  static const double x3 = 1.8;	        /* ??? */
  static const double x5 = M_PI_2;	/* pi/2 */
  static const double x6 = M_PI;	/* pi */
  static const double x7 = M_PI*2.0;    /* 2 pi */
  static const double x8 = M_PI/180.0;	/* radians per degree */

  /* Computed constants */

  static int initialized = 0;
  static double x9, y0, y1, y2, y3, y4, y5;

  /* Temporary variables */

  double a, b, c, d, e, f, g, h, i, j, k, l, m, n, o, p, r, s, t;
  int q;
  
  /* The following is translated from a completely undocumented BASIC
     program.  The following are the (approximate) values that should
     be assigned by the following test case:

     lat1  = 0.1875
     long1 = -89.7916667
     lat2  = 0.1875
     long1 = 89.7916667

     distance = 19954.90 km
     heading = 48.01 degrees
     */

  if ( !initialized )
    {
      a = x0*x0;
      b = x1*x1;
      b = 1.0 + (a-b)/b;
      c = sqrt(b);
      a = a/x1;
      x9 = (1.0 + 1.0/(b*c)) * a / 2.0;
      y0 = a - x9;
      y1 = (x0+a)/2.0;
      y2 = a-y1;
      y3 = (2.0*x9/x0-1.0)*x6;
      y4 = x0-x9;
      y5 = x7*(1.0-x9/x0);
      y5 = y5*y5;

      initialized = 1;
    }

  a = long1 * x8;
  b = lat1 * x8;
  c = long2 * x8;
  d = lat2 * x8;

  e = c-a;
  f = sin(b);
  g = sin(d);
  h = cos(b);
  i = cos(d);
  j = cos(e);

  k = f*g+h*i*j;
  m = lfunc(&k);
  if ( fabs(k) < 1.0 )
    n = (g*h-i*f*j)/sqrt(1.0-k*k);

  k = n;
  g = lfunc(&k);
  i = m/4;
  j = -i/2;
  p = 0.0;

  for ( q = 1 ; q <= 4 ; q++ )
    {
      j += i;
      k = cos(j)*f + sin(j)*h*n;
      l = lfunc(&k);
      r = 0.0;
      if ( l != 0.0 )
	r = h*sin(g)/sin(l);
      
      s = r*x5;
      if ( fabs(r) < 1.0 )
	s = atan(r/sqrt(1.0-r*r));
      
      r = cos(l+l);
      t = x9+y0*r;
      r = y1+y2*r;
      p += (t+r)/2.0 + (t-r)/2.0 * cos(s+s);
    }

  f = p/4;
  h = 0.0;
  i = m-y3;
  if ( i > 0.0 )
    h = i*i*(f-x9)/y5;

  i = sin(x6*(x0-f)/y4);
  j = y3*(1.0-x2*i);
  
  if ( m > j )
    h += x3*i*sin(x6*sqrt((x6-m)/(x6-j)));
  
  f = (f-h)*m;

  if ( f < 0.005 || f > 20003.5 )
    g = 0.0;
  else if ( e*(x6-fabs(e)) < 0.0 )
    g = x7-g;

  *distance = f;
  *heading = g/x8;
}