/* *********************************************************
 * *                                                       *
 * *               LowLevel Math Routines                  *
 * *                                                       *
 * *********************************************************
 * file: libs/user/lowmath.c
 * 
 *     COPYRIGHT (C) 1988, 1994, 1998, 1999, 2000, 2003, 2005  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 <stdio.h>
#include <math.h>
#include "sigtypes.h"
#include "user_lib.h"

/* ------------------------------
 * The IPOWER_OF_TWO() function gives the largest unsigned number
 * which is less than or equal to the argument, and is a power of two.
 * This can also be interpreted as the highest bit set in the arg.
 *
 * The ILOG_BASE_TWO() function is similar, but returns the number of
 * the highest bit set, or -1 if none.
 *
 * The NUM_BITS_SET() function returns the total number of bits that
 * are set in the argument.
 */
UINT4 ipower_of_two( UINT4 ivalue_a )
{
  SINT4 iLog = ilog_base_two( ivalue_a ) ;
  if( iLog < 0 ) return( 0 ) ;
  else           return( 1 << iLog ) ;
}

SINT4 ilog_base_two( UINT4 ivalue_a )
{
  SINT4 iLog = -1 ; UINT4 ivalue = ivalue_a ;
  while( ivalue ) { iLog++ ; ivalue >>= 1 ; }
  return( iLog ) ;
}

SINT4 num_bits_set( UINT4 ivalue_a )
{
  SINT4 iBits = 0 ; UINT4 ivalue = ivalue_a ;
  while( ivalue ) { if( ivalue & 1 ) iBits++ ; ivalue >>= 1 ; }
  return( iBits ) ;
}

/* ------------------------------
 * Routines to return the full 64-bits of a 32x32 product.  These
 * routines do not make any assumptions about how the CPU handles
 * integer overflows, i.e., they never perform any multiplication
 * that could possibly overflow.
 */
UINT4 iMpyU64( UINT4 iarg1_a, UINT4 iarg2_a, UINT4 *iLowProd_a )
{
  UINT4 ipll = (iarg1_a & 0xFFFF) * (iarg2_a & 0xFFFF) ;
  UINT4 iphl = (iarg1_a >> 16   ) * (iarg2_a & 0xFFFF) ;
  UINT4 iplh = (iarg1_a & 0xFFFF) * (iarg2_a >> 16   ) ;
  UINT4 iphh = (iarg1_a >> 16   ) * (iarg2_a >> 16   ) ;

  UINT4 ic32 = (ipll >> 16) + (iphl & 0xFFFF) + (iplh & 0xFFFF) ;
  UINT4 ip00 = ipll + (iphl << 16) + (iplh << 16) ;
  UINT4 ip32 = iphh + (iphl >> 16) + (iplh >> 16) ;

  if( iLowProd_a ) *iLowProd_a = ip00 ;
  return( ip32 + (ic32 >> 16) ) ;
}

/* ------------------------------
 * Euclid's algorithm for computing the greatest common divisor of two
 * integers.  This is probably the best documented algorithm in all of
 * human history; so no explaination is given here.
 */
UINT4 iEuclidGCD( UINT4 iM_a, UINT4 iN_a )
{
  if( (iM_a == 0) || (iN_a == 0) ) return( 0 ) ;
  else {
    UINT4 iM = iM_a, iN = iN_a, iRem = 1 ;
    while( iRem > 0 ) {
      iRem = (iM % iN) ; iM = iN ; iN = iRem ;
    }
    return( iM ) ;
  }
}

UINT4 iEuclidLCM( UINT4 iM_a, UINT4 iN_a )
{
  UINT4 iProd = ( iM_a * iN_a ) ;
  return( iProd / iEuclidGCD( iM_a, iN_a ) ) ;
}

/* ------------------------------
 * Given a floating point number F, determine the smallest positive
 * integer Q such that the product QF is also an integer.  In other
 * words, determine the value Q such that F = P/Q for some integer P.
 *
 * If no suitable integer exists within the indicated maximum range,
 * then return the negative of the Q that produced a product QF having
 * the smallest fractional offset from an integer.
 */
SINT4 iRationalDenom( FLT8 fArg_a, SINT4 iMaxDenom_a )
{
  SINT4 iTry, iDenom=0 ; FLT8 fInt, fTol = 5E-3 / (FLT8)iMaxDenom_a ;
  FLT8 fFracArg = modf( fabs(fArg_a), &fInt ), fBestFrac = 1.0 ;
  for( iTry=1 ; iTry <= iMaxDenom_a ; iTry++ ) {
    FLT8 fProd = fFracArg * (FLT8)iTry ;
    FLT8 fFrac = fabs( modf( (fProd + 0.5), &fInt ) - 0.5 ) ;
    if( fFrac < fTol      ) { iDenom =  iTry ; break ; }
    if( fFrac < fBestFrac ) { iDenom = -iTry ; fBestFrac = fFrac ; }
  }
  return( iDenom ) ;
}

/* ------------------------------
 * Collection of routines to use when QSORT'ing some common arrays.
 */
#define QSORTMAC( TYPE )			\
{						\
  TYPE val1 = *(TYPE *)arg1_a ;			\
  TYPE val2 = *(TYPE *)arg2_a ;			\
  if     ( val2 < val1 ) return( -1 ) ;		\
  else if( val1 < val2 ) return(  1 ) ;		\
  else                   return(  0 ) ;		\
}
int  iQsortCmpFLT4( const void *arg1_a, const void *arg2_a ) QSORTMAC( FLT4 )
int  iQsortCmpFLT8( const void *arg1_a, const void *arg2_a ) QSORTMAC( FLT8 )

int iQsortCmpSINT1( const void *arg1_a, const void *arg2_a ) QSORTMAC( SINT1 )
int iQsortCmpSINT2( const void *arg1_a, const void *arg2_a ) QSORTMAC( SINT2 )
int iQsortCmpSINT4( const void *arg1_a, const void *arg2_a ) QSORTMAC( SINT4 )

int iQsortCmpUINT1( const void *arg1_a, const void *arg2_a ) QSORTMAC( UINT1 )
int iQsortCmpUINT2( const void *arg1_a, const void *arg2_a ) QSORTMAC( UINT2 )
int iQsortCmpUINT4( const void *arg1_a, const void *arg2_a ) QSORTMAC( UINT4 )

/* ------------------------------
 * Function : kth_smallest()
 *       In : array of elements, # of elements in the array, rank k
 *      Out : one element from the original array
 *      Job : find the kth smallest element in the array
 *   Notice : 1) Use the FMEDIAN() or IMEDIAN() macros to get the median
 *            2) The input array is clobbered
 *
 *       Author: Wirth, Niklaus 
 *        Title: Algorithms + data structures = programs 
 *    Publisher: Englewood Cliffs: Prentice-Hall, 1976 
 *  Description: 366 p. 
 *       Series: Prentice-Hall Series in Automatic Computation 
 */
#define KTHSMALLEST( TYPE, NAME )				\
TYPE NAME( TYPE data_a[], SINT4 iSize_a, SINT4 kth_a )		\
{								\
  SINT4 ilo = 0, ihi = MAX( 1, iSize_a ) - 1 ;			\
  SINT4 kth = MAX( 0, MIN( ihi, kth_a ) ) ;			\
								\
  while( ilo < ihi ) {						\
    SINT4 ii = ilo, ij = ihi ; TYPE xVal = data_a[ kth ] ;	\
    do {							\
      while( data_a[ii] < xVal ) ii++ ;				\
      while( data_a[ij] > xVal ) ij-- ;				\
      if( ii <= ij ) {						\
	SCALARSWAP( data_a[ii], data_a[ij], TYPE ) ;		\
	ii++ ; ij-- ;						\
      }								\
    } while( ii <= ij ) ;					\
    if( ij < kth ) ilo = ii ;					\
    if( ii > kth ) ihi = ij ;					\
  }								\
  return( data_a[ kth ] ) ;					\
}
KTHSMALLEST(  FLT4, fKthSmallest ) /* Expand the macro to create */
KTHSMALLEST( SINT4, iKthSmallest ) /*   the actual functions. */
