/* **********************************************************************
 * *	                                                                *
 * *                 Spherical Trigonometry Routines                    * 
 * *                                                                    *
 * **********************************************************************
 * File: libs/user/sphere.c
 *
 *      COPYRIGHT (c) 1990, 1991, 1996, 1999, 2000, 2001, 2004  BY
 *          SIGMET INCORPORATED, WESTFORD MASSACHUSETTS, U.S.A.  
 *
 * THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY BE USED AND  COPIED
 * ONLY  IN  ACCORDANCE WITH  THE  TERMS  OF  SUCH  LICENSE  AND WITH THE
 * INCLUSION OF THE ABOVE COPYRIGHT NOTICE.  THIS SOFTWARE  OR  ANY OTHER
 * COPIES  THEREOF MAY NOT BE PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY
 * OTHER PERSON.  NO TITLE TO AND OWNERSHIP OF  THE  SOFTWARE  IS  HEREBY
 * TRANSFERED. 
 */
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "sigtypes.h"
#include "user_lib.h"

#define RAD90  (FPI / 2.0)
#define RAD180 (FPI      )
#define RAD360 (FPI * 2.0)

static void TriangleSphereSas
(struct Triangle8 *pTriangle_a);

/* ====================================================================
 * Routine to convert a starting point, and a polar offset to an ending
 * point in earth coordinates.  This routine will not work if the starting
 * point is at either the North or South poles.  Also the range must be
 * less than half way around the earth.
 *
 * Entry parameters:
 *     frange_a - Range along the earth's surface, in kilometers
 *     fangle_a - Angle of the polar vector in radians, clockwise from north
 *
 * Exit parameters:
 *     fend_a - End point position in radians
 */
void EarthFromPolar
(volatile const struct latlon8 *Start_a,   /* Origin position in radians */
 volatile const struct D2Polar8 *pPolar_a,
 volatile struct latlon8 *End_a)
{
  UINT2 leasterly ;
  struct Triangle8 Triangle;

  /* ---------------------------------------------------------------------
   * Consider the spherical triangle consisting of the Origin point (called 2),
   * the end point (called 1) and the North pole (called 3). 
   * We know side 1, angle 2, and side 3.  It is then possible to solve for the
   * other parts of the triangle.  Side 2 gives us the latitude of the end 
   * point, and angle 3 gives us the difference in longitudes between the
   * two points.  Note that all angles here are in radians, and all side
   * lengths are in radians of arc from the center of the earth.
   *
   *         3 (at north pole)
   *         |\
   *         | \
   *         |  \(side 2)
   *         |   \
   *         |    \
   * (side 1)|     \
   *  (known)|     / 1
   *         |    /
   *         |   /
   *         |  /(side 3(c)
   *         | / (known)
   *         |/
   *         2
   *      (known)
   */

  /* Change units. */
  Triangle.fSide3  = pPolar_a->fRange/FEARTH_MEAN_RADIUS ;
  Triangle.fAngle2 = pPolar_a->fAz;
  /*
   * Clear up the degenerate case of the range being zero.
   */
  if( Triangle.fSide3 == 0.0 )
    {
      *End_a = *Start_a;
      return ;
    }

  /* Negative range is the same as positive in the other direction */
  if( Triangle.fSide3 < 0.0 )
    {
      Triangle.fSide3 = -Triangle.fSide3;
      Triangle.fAngle2 = Triangle.fAngle2 + RAD180;
      if (Triangle.fAngle2 >= RAD360) Triangle.fAngle2 -= RAD360;
    }
  
  Triangle.fSide1 = RAD90 - Start_a->fLat;

  /* Constrain the triangle to have angles of less than 180 degrees
   */
  if( Triangle.fAngle2 > RAD180 )
    {
      Triangle.fAngle2 = RAD360 - Triangle.fAngle2 ;
      leasterly = FALSE ;
    }
  else
    {
      leasterly = TRUE ;
    }
  
  TriangleSphereSas( &Triangle ) ;
  
  /* Use the pieces of the triangle to get the position.  Note that all longitudes
   * should be forced to be in the range -180 to +180.
   */
  memset( (void*)End_a, 0, sizeof(*End_a) );  /* Cast discards volatile */
  End_a->fLat = RAD90 - Triangle.fSide2 ;
  if( leasterly )
    {
      End_a->fLon = Start_a->fLon + Triangle.fAngle3 ;
      if( End_a->fLon > RAD180 ) End_a->fLon -= RAD360 ;
    }
  else
    {
      End_a->fLon = Start_a->fLon - Triangle.fAngle3 ;
      if( End_a->fLon < -RAD180 ) End_a->fLon += RAD360 ;
    }
}
/* ====================================================================
 * Old format routine.
 */
void polar_to_earth
  (volatile const struct latlon8 *Start_a,
   FLT8 fRange_a,
   FLT8 fAngle_a,  /* Angle, in radians, range 0-2 PI */
   volatile struct latlon8 *End_a)
{
  struct D2Polar8 Polar;
  
  memset( &Polar, 0, sizeof(Polar) );
  Polar.fRange = fRange_a;
  Polar.fAz    = fAngle_a;

  EarthFromPolar( Start_a, &Polar, End_a);

}

/* ====================================================================
 * Given the latitude and longitude of two points on the earth, compute
 * the range between them, and the bearings from each point to the other.
 * This routine will not work if either point
 * point is at either the North or South poles.  The two points can be at
 * the same place.
 *
 * Entry parameters:
 *     fearth1_a  - Origin position in radians, north/east positive
 *     fearth2_a  - End point 
 *
 * Exit parameters:
 *     frange_a  - Range along the earth's surface, in kilometers
 *     faz1to2_a - Bearing from point one to point two, set to 0 if range is 0.
 *     faz2to1_a - Bearing from point two to point one
 */
void earth_to_polar
  (volatile const struct latlon8 *Earth1_a,
   volatile const struct latlon8 *Earth2_a,
   volatile FLT8 *frange_a,
   volatile FLT8 *faz1to2_a,
   volatile FLT8 *faz2to1_a)
{
  struct D2Polar8 Polar;

  PolarFromEarth( Earth1_a, Earth2_a, &Polar );
  *frange_a  = Polar.fRange;
  *faz1to2_a = Polar.fAz;

  PolarFromEarth( Earth2_a, Earth1_a, &Polar );
  *faz2to1_a = Polar.fAz;

}
/* ====================================================================
 * Given the latitude and longitude of two points on the earth, compute
 * the range between them, and the bearing from point1 to point2.
 * This routine will not work if either point
 * point is at either the North or South poles.  The two points can be at
 * the same place.
 *
 * Entry parameters:
 *     fearth1_a  - Origin position in radians, north/east positive
 *     fearth2_a  - End point 
 *
 * Exit parameters:
 *     frange_a  - Range along the earth's surface, in kilometers
 *     faz1to2_a - Bearing from point one to point two, set to 0 if range is 0.
 */
void PolarFromEarth
  (volatile const struct latlon8 *Earth3_a,
   volatile const struct latlon8 *Earth1_a,
   volatile struct D2Polar8 *pPolar_a )
{
  UINT2 l3to1goes_east ;
  struct Triangle8 Triangle;

  /* ---------------------------------------------------------------------
   * Consider the spherical triangle consisting of point 3,
   * point 1 and the North pole (called 2). 
   * We know side 1, angle 2, and side 3.  
   * It is then possible to solve for the other parts of the triangle.  
   * Note that all angles here are in radians, and all side
   * lengths are in radians of arc from the center of the earth.
   *
   *         2(at north pole)
   *         |\ (known)
   *         | \
   *         |  \(side 3)
   *         |   \ (known)
   *         |    \
   * (side 1)|     \
   * (known) |     / 1
   *         |    /
   *         |   /
   *         |  /(side2)
   *         | /
   *         |/
   *         3
   */
  Triangle.fSide1 = RAD90 - Earth3_a->fLat;
  Triangle.fSide3 = RAD90 - Earth1_a->fLat;
  Triangle.fAngle2 = Earth1_a->fLon - Earth3_a->fLon ;
  if( Triangle.fAngle2 < 0.0 ) Triangle.fAngle2 += RAD360 ;

  /* Constrain the triangle to have angles of less than 180 degrees
   */
  if( Triangle.fAngle2 > RAD180 )
    {
      Triangle.fAngle2 = RAD360 - Triangle.fAngle2 ;
      l3to1goes_east = FALSE ;
    }
  else
    {
      l3to1goes_east = TRUE ;
    }
  
  TriangleSphereSas( &Triangle ) ;
  
  /* Use the pieces of the triangle to get the position
   */
  memset( (void*)pPolar_a, 0, sizeof(*pPolar_a) );  /* Cast discards qualifiers */
  pPolar_a->fRange = FEARTH_MEAN_RADIUS * Triangle.fSide2 ;
  if( l3to1goes_east )
    {
      pPolar_a->fAz = Triangle.fAngle3 ;
    }
  else
    {
      pPolar_a->fAz = RAD360 - Triangle.fAngle3 ;
    }
}

/* ====================================================================
 * Spherical trigonometry routines.
 * Given a triangle on the surface of a sphere, and a known side-angle-side,
 * solve for the other side and two angles.  Note that sideN is opposite
 * angleN.  Angles are all in radians, and sides are also in radians of
 * angle from the center of the sphere.  Note that all sides must be positive,
 * and less than 180 degrees.  Some cases with a side equal to 180 degrees
 * will work.  All angles must be in the range 0 through 180
 * degrees inclusive.
 *
 * WARNING: There are several somewhat degenerate cases:
 * (a) angle2=180, side1=90, side=90 ==> resulting angles unclear, set to 90
 * (b) angle2=0,   side1=90, side=90 ==> resulting angles unclear, set to 90
 *
 * Entry parameters:
 *     fside1_a  - Length of side
 *     fside3_a  - Length of side
 *     fangle2_a - Angle of included angle
 *
 * Exit parameters:
 *     fside2_a  - Length of side
 *     fangle1_a - Angle opposite side 1
 *     fangle3_a - Angle oppisite side 3
 */
static void TriangleSphereSas
  (struct Triangle8 *pTriangle_a)
{
  FLT8 f_cot_half2, f_halfside3plus1, f_halfside3minus1 ;
  FLT8 fsin_half2, fcos_half2, f_halfangle3plus1, f_halfangle3minus1 ;
  FLT8 ftemp1a, ftemp1b, ftemp2a, ftemp2b, fcos_side2 ;

  /* ---------------------------------------------------------------------
   *
   *         3
   *         |\
   *         | \
   *         |  \(side 2)
   *         |   \
   *         |    \
   * (side 1)|     \
   * (known) |     / 1
   *         |    /
   *         |   /
   *         |  /(side 3)
   *         | / (known)
   *         |/
   *         2
   *      (known)
   */

  /* Constrain the triangle to have angles of less than 180 degrees.
   */
  fsin_half2 = sin(0.5 * pTriangle_a->fAngle2) ;
  fcos_half2 = cos(0.5 * pTriangle_a->fAngle2) ;
  f_halfside3plus1  = (pTriangle_a->fSide3 + pTriangle_a->fSide1) / 2.0 ;
  f_halfside3minus1 = (pTriangle_a->fSide3 - pTriangle_a->fSide1) / 2.0 ;

  /* Check some degenerate cases, where the known angle is either zero, or
   * 180 degrees.
   */
  if( fsin_half2 == 0.0 )
    {
      if( pTriangle_a->fSide3 > pTriangle_a->fSide1 ) /* Near zero degrees: */
	{
	  pTriangle_a->fAngle3 = RAD180 ;
	  pTriangle_a->fAngle1 = 0.0 ;
	}
      else if( pTriangle_a->fSide3 == pTriangle_a->fSide1 )
	{
	  pTriangle_a->fAngle3 = RAD90 ;
	  pTriangle_a->fAngle1 = RAD90 ;
	}
      else
	{
	  pTriangle_a->fAngle3 = 0.0 ;
	  pTriangle_a->fAngle1 = RAD180 ;
	}
      f_halfangle3plus1  = (pTriangle_a->fAngle3 + pTriangle_a->fAngle1) / 2.0 ;
      f_halfangle3minus1 = (pTriangle_a->fAngle3 - pTriangle_a->fAngle1) / 2.0 ;

    }
  else if( fsin_half2 == 1.0 )
    {
      if( (pTriangle_a->fSide3 + pTriangle_a->fSide1) > RAD180 ) /* Near 180 degrees */
	{
	  pTriangle_a->fAngle3 = RAD180 ;
	  pTriangle_a->fAngle1 = RAD180 ;
	}
      else if( (pTriangle_a->fSide3 + pTriangle_a->fSide1) == RAD180 )
	{
	  pTriangle_a->fAngle3 = RAD90 ;
	  pTriangle_a->fAngle1 = RAD90 ;
	}
      else
	{
	  pTriangle_a->fAngle3 = 0.0 ;
	  pTriangle_a->fAngle1 = 0.0 ;
	}
      f_halfangle3plus1  = (pTriangle_a->fAngle3 + pTriangle_a->fAngle1) / 2.0 ;
      f_halfangle3minus1 = (pTriangle_a->fAngle3 - pTriangle_a->fAngle1) / 2.0 ;
    }
  else
    {
      /* Using Napier's analogies:
       * Tan( 1/2 (C+B)) = cos( 1/2 (c-b) ) * Cot( 1/2 * A ) / cos( 1/2 (c+b) )
       * Tan( 1/2 (C-B)) = sin( 1/2 (c-b) ) * cot( 1/2 * A ) / sin( 1/2 (c+b) )
       */
      f_cot_half2 = fcos_half2/fsin_half2 ;

      /* We should add protection against atan2(0,0)! */

      ftemp1a = cos(f_halfside3minus1) * f_cot_half2 ;
      ftemp1b = cos(f_halfside3plus1) ;
      f_halfangle3plus1 = atan2( ftemp1a, ftemp1b ) ;

      ftemp2a = sin(f_halfside3minus1) * f_cot_half2 ;
      ftemp2b = sin(f_halfside3plus1) ;
      f_halfangle3minus1 = atan2( ftemp2a, ftemp2b ) ;

      pTriangle_a->fAngle3 = f_halfangle3plus1 + f_halfangle3minus1 ;
      pTriangle_a->fAngle1 = f_halfangle3plus1 - f_halfangle3minus1 ;

      /* Note that at this point, both angle 1 and 3 should be positive, in the
       * range 0 through 180 degrees inclusive.
       */
    }
	
  /* Finally, use law of cosines to fill in the missing side.
   */
  fcos_side2 = ( cos(pTriangle_a->fSide1) * cos(pTriangle_a->fSide3) ) +
    ( sin(pTriangle_a->fSide1) * sin(pTriangle_a->fSide3) * cos(pTriangle_a->fAngle2) ) ;

  if( fcos_side2 <= -1.0 )
    pTriangle_a->fSide2 = RAD180 ;
  else if( fcos_side2 >= 1.0 )
    pTriangle_a->fSide2 = 0.0 ;
  else
    pTriangle_a->fSide2 = acos( fcos_side2 ) ;
}
