/* **********************************************************************
 * *	                                                                *
 * *         Routines to Help With Angle Conversions and Tests          *
 * *                                                                    *
 * **********************************************************************
 * File: libs/user/angle.c
 *
 *     COPYRIGHT (c) 1988, 1993, 1996, 1998, 2000, 2002, 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 <stdlib.h>

#include "sigtypes.h"
#include "user_lib.h"

/* ------------------------------
 * These functions convert a 16/32-bit binary angle into a real number
 * in degrees.  Result is between -180 and +180.
 */
FLT4 fDegFromBin2( BIN2 ibinang_a )
{
  FLT4 fresult = fPDegFromBin2( ibinang_a ) ;
  if (fresult > 180.0) fresult -= 360.0 ;
  return( fresult ) ;
}
FLT8 fDegFromBin4( BIN4 ibinang_a )
{
  FLT8 fresult = fPDegFromBin4( ibinang_a ) ;
  if (fresult > 180.0) fresult -= 360.0 ;
  return( fresult ) ;
}

/* ------------------------------
 * These functions convert a 16/32-bit binary angle into a real valued
 * 'elevation angle' in degrees.  Result is between -90 and +270.
 */
FLT4 fElDegFromBin2( BIN2 ibinang_a )
{
  FLT4 fresult = fPDegFromBin2( ibinang_a ) ;
  if (fresult > 270.0) fresult -= 360.0 ;
  return( fresult ) ;
}
FLT8 fElDegFromBin4( BIN4 ibinang_a )
{
  FLT8 fresult = fPDegFromBin4( ibinang_a ) ;
  if (fresult > 270.0) fresult -= 360.0 ;
  return( fresult ) ;
}

/* ------------------------------
 * These functions convert a 16/32-bit binary angle into a positive
 * real number in degrees.  Result is between 0 and +360.
 */
FLT4 fPDegFromBin2( BIN2 ibinang_a )
{
  FLT4 fresult = ((FLT4)ibinang_a ) / (65536.0 / 360.0) ;
  return( fresult ) ;
}
FLT8 fPDegFromBin4( BIN4 ibinang_a )
{
  FLT8 fresult = ((FLT8)ibinang_a ) / (4294967296.0 / 360.0) ;
  return( fresult ) ;
}

/* ------------------------------
 * These functions convert a 16/32-bit binary angle into a real number
 * in radians.  Result is between -PI and +PI.
 */
FLT4 fRadianFromBin2( BIN2 ibinang_a )
{
  FLT4 fresult = ((FLT4)(SINT2)ibinang_a ) / (32768.0 / FPI) ;
  return( fresult ) ;
}
FLT8 fRadianFromBin4( BIN4 ibinang_a )
{
  FLT8 fresult = ((FLT8)(SINT4)ibinang_a ) / (2147483648.0 / FPI) ;
  return( fresult ) ;
}

/* ------------------------------
 * These functions convert a real angle in degrees to a 16/32-bit
 * binary angle.  Note that the 32-bit routine deals with sign by
 * processing 1/2 the number and then shifting
 */
BIN2 iBin2FromFDeg( FLT4 fangle_a )
{
  return( IBIN2FROMFDEG( fangle_a ) ) ;
}
BIN4 iBin4FromFDeg( FLT8 fangle_a )
{
  BIN4 iresult ; FLT8 fangle = fmod( fangle_a, 360.0 ) ;
  if( fangle < 0.0 ) fangle += 360.0 ;
  iresult = ((UINT4)NINT( (1073741824.0 / 180.0) * fangle )) << 1 ;
  return( iresult ) ;
}

/* ------------------------------
 * These functions convert a real angle in radians to a 16/32-bit
 * binary angle.  Note that the 32-bit routine deals with sign by
 * processing 1/2 the number and then shifting
 */
BIN2 iBin2FromRadian( FLT4 fRadian_a )
{
  return( IBIN2FROMRADIAN( fRadian_a ) ) ;
}
BIN4 iBin4FromRadian( FLT8 fRadian_a )
{
  BIN4 iresult ; FLT8 fradian = fmod( fRadian_a, (2.0 * FPI) ) ;
  if( fradian < 0.0 ) fradian += (2.0 * FPI) ;
  iresult = ((UINT4)NINT( (1073741824.0 / FPI) * fradian )) << 1 ;
  return( iresult ) ;
}

/* ------------------------------
 * The following functions return the binary angle which is the
 * midpoint between the first and second args.
 */

/* The NARROW mid point is defined as lying in the smaller sector
 * defined by the two angles.  Thus, the order of the arguments does
 * not affect the returned value.
 */
BIN2 iBin2NarrowMidPt( BIN2 ia_a, BIN2 ib_a )
{
  SINT2  idiff = (ib_a - ia_a) ;
  BIN2 imiddle = ia_a + (BIN2)(idiff >> 1) ;
  return( imiddle ) ;
}
/* The ORDERED mid point is defined as lying in the increasing angle
 * sector beginning at the first angle and ending at the second.
 * Thus, the order of the arguments does affect the returned value.
 */
BIN2 iBin2OrderedMidPt( BIN2 ia_a, BIN2 ib_a )
{
  UINT2  idiff = (ib_a - ia_a) ;
  BIN2 imiddle = ia_a + (BIN2)(idiff >> 1) ;
  return( imiddle ) ;
}

/* ------------------------------
 * Function to determine whether three binary angles are "in a row",
 * i.e., the middle one is in the increasing angle interval from the
 * first to the third.  If the first and third arguments are identical
 * then the test will fail unless the second arg is also identical.
 */
UINT1 lbin2_in_a_row( BIN2 ia_a, BIN2 ib_a, BIN2 ic_a )
{
  UINT2 ia_to_b = ((UINT2)ib_a) - (UINT2)ia_a ;
  UINT2 ia_to_c = ((UINT2)ic_a) - (UINT2)ia_a ;
  UINT1 lresult = ( ia_to_b <= ia_to_c ) ;
  return( lresult ) ;
}

/* ------------------------------
 * Convert Binary-Coded-Decimal value to a binary angle.
 */
BIN2 iBin2FromBcd( UINT2 ibcd_a )
{
  BIN2  iresult = iBin2FromFDeg
    (100.0 * (FLT4)((ibcd_a>>12) & 3 ) +
      10.0 * (FLT4)((ibcd_a>> 8) &15 ) +
       1.0 * (FLT4)((ibcd_a>> 4) &15 ) +
       0.1 * (FLT4)( ibcd_a      &15 ) ) ;

  return( iresult ) ;
}

/* ------------------------------
 * Return 0 if a 16-bit binary angle is strictly within an increasing
 * angle sector ; else return -1 or +1 according to which edge we are
 * closer to.
 */
SINT4 binside( BIN2 left_a, BIN2 test_a, BIN2 right_a )
{
  UINT2 ii, ij ;

  ii = (test_a  - left_a) ;
  ij = (right_a - left_a) ;
  if( (ii != 0) && (ii < ij) ) return(0) ;

  ii = (test_a - right_a) ;
  ij = (left_a - test_a ) ;
  if( ii < ij ) return(1) ; else return(-1) ;
}

/* ------------------------------
 * Return 0 if a 16-bit binary angle is encompased by an increasing
 * angle sector ; else return -1 or +1 according to which edge we are
 * closer to.  This is similar to binside() except that the sector
 * limits are included in the valid region.
 */
SINT4 binEncompased( BIN2 left_a, BIN2 test_a, BIN2 right_a )
{
  UINT2 ii, ij ;

  ii = (test_a  - left_a) ;
  ij = (right_a - left_a) ;
  if( ii <= ij ) return(0) ;

  ii = (test_a - right_a) ;
  ij = (left_a - test_a ) ;
  if( ii < ij ) return(1) ; else return(-1) ;
}
