/*
**  gcdist.c 1.1	Solaris 2.3 Unix	940907	SIO/ODF	fmd
**
**  Functions for computing great-circle distances and positions.
**
**  References:
**  ==========
**
**  Bowditch, N.; "American Practical Navigator, Vol. I."; Defense Mapping
**  Agency Hydrographic Center, 1977. Pages 1258-1263.
*/
#include <odflib.h>
#include <math.h>

#ifndef FORTRAN

#define KmPerDegree		111.12000071117
#define	DegreesPerKm		(1.0/KmPerDegree)
#define PI			M_PI
#define TwoPI			(M_PI+M_PI)
#define HalfPI			M_PI_2
#define RadiansPerDegree	(PI/180.0)
#define	DegreesPerRadian	(180.0/PI)
#define copysign(x,y)	(((y)<0.0)?-fabs(x):fabs(x))
#define NGT1(x)		(fabs(x)>1.0?copysign(1.0,x):(x))
#define ArcCos(x)	(fabs(x)>1?quiet_nan():acos(x))
#define hav(x)		((1.0-cos(x))*0.5)		/* haversine */
#define ahav(x)		(ArcCos(NGT1(1.0-((x)*2.0))))	/* arc haversine */
#define sec(x)		(1.0/cos(x))			/* secant */
#define csc(x)		(1.0/sin(x))			/* cosecant */

/*
** GreatCircleDist() --
**
** Compute great-circle distance and course from starting
** and ending positions.
**
** Given a starting position (decimal latitude and longitude) and
** an ending position (decimal latitude and longitude), compute
** the great-circle distance (kilometers) and initial course.
** This is the inverse function to GreatCirclePos().
*/
double			/* <- Great-circle distance (km) */
GreatCircleDist(slt, slg, xlt, xlg, course)
	double	slt;	/* -> starting decimal latitude (-S) */
	double	slg;	/* -> starting decimal longitude(-W) */
	double	xlt;	/* -> ending decimal latitude (-S) */
	double	xlg;	/* -> ending decimal longitude(-W) */
	double	*course;/* <- initial course (degrees) */
{
	double	c, d, dLo, L1, L2, coL1, coL2, l;
    /*
    **  Translate longitude to starting position
    */
	if (slg < 0.0) slg += 360.0;
	if (xlg < 0.0) xlg += 360.0;
	dLo  = (xlg-slg)*RadiansPerDegree;
	if (fabs(dLo) > PI)
	    dLo += (dLo < 0.0) ? TwoPI : -TwoPI;
	slt *= RadiansPerDegree;
	xlt *= RadiansPerDegree;
	L1   = slt;
	L2   = xlt;
	if (dLo == 0.0 && L1 == L2) {
	    if (course)
		*course = 0.0;
	    return (0.0);
	}
	l    = L2-L1;
	coL1 = (HalfPI-slt);
	coL2 = (HalfPI-xlt);
    /*
    **  Calculate great-circle distance in radians.
    */
	d    = ahav(hav(dLo)*cos(L1)*cos(L2)+hav(l));
    /*
    **  Calculate initial course in radians.
    */
	if (course) {
	    c = ahav(sec(L1)*csc(d)*(hav(coL2)-hav(d-coL1)));
	    if (isnan(c))
		c = 0.0;
	    if (dLo < 0.0)
		c = -c;
	    if (c < 0.0) c += TwoPI;
	    *course = c*DegreesPerRadian;
		/* initial course angle in degrees  */
	}
    /*
    **  Return great-circle distance in kilometers.
    */
	return (d*DegreesPerRadian*KmPerDegree);

} /* GreatCircleDist() */

/*
**  GreatCirclePos() --
**
**  Compute ending position from course and great-circle distance.
**
**  Given a starting latitude (decimal), the initial great-circle
**  course and a distance along the course track, compute the ending
**  position (decimal latitude and longitude).
**  This is the inverse function to GreatCircleDist).
*/
void
GreatCirclePos(dist, course, slt, slg, xlt, xlg)
	double	dist;	/* -> great-circle distance (km) */
	double	course;	/* -> initial great-circle course (degrees) */
	double	slt;	/* -> starting decimal latitude (-S) */
	double	slg;	/* -> starting decimal longitude(-W) */
	double	*xlt;	/* <- ending decimal latitude (-S) */
	double	*xlg;	/* <- ending decimal longitude(-W) */
{
	double	c, d, dLo, L1, L2, coL1, coL2, l;

	if (dist > KmPerDegree*180.0) {
	    course -= 180.0;
	    if (course < 0.0) course += 360.0;
	    dist    = KmPerDegree*360.0-dist;
	}
	if (course > 180.0) course -= 360.0;
	c    = course*RadiansPerDegree;
	d    = dist*DegreesPerKm*RadiansPerDegree;
	L1   = slt*RadiansPerDegree;
	slg *= RadiansPerDegree;
	coL1 = (90.0-slt)*RadiansPerDegree;
	coL2 = ahav(hav(c)/(sec(L1)*csc(d))+hav(d-coL1));
	L2   = HalfPI-coL2;
	l    = L2-L1;
	if ((dLo=(cos(L1)*cos(L2))) != 0.0)
	    dLo  = ahav((hav(d)-hav(l))/dLo);
	if (c < 0.0) dLo = -dLo;
	slg += dLo;
	if (slg < -PI)
	    slg += TwoPI;
	else if (slg > PI)
	    slg -= TwoPI;

	*xlt = L2*DegreesPerRadian;
	*xlg = slg*DegreesPerRadian;

} /* GreatCirclePos() */

#else  /* FORTRAN */
/*
**  These routines are Fortran-callable.
**
**  Note that 'E' is negative longitude.
*/
void
gcdist_(xlt, xlg, slt, slg, dist, course)
	float	*xlt, *xlg, *slt, *slg, *dist, *course;
{
	double	c, GreatCircleDist();
	*dist   = GreatCircleDist(*slt, -(*slg), *xlt, -(*xlg), &c);
	*course = c;
}

void
gcpos_(dist, course, slt, slg, xlt, xlg)
	float	*dist, *course, *slt, *slg, *xlt, *xlg;
{
	double	dxlt, dxlg;
	GreatCirclePos(*dist, *course, *slt, -(*slg), &dxlt, &dxlg);
	*xlt = dxlt;
	*xlg = -dxlg;
}

#endif /* FORTRAN */