/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : Math.cc                                                       */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
static char Math_id[] = "$Header: /usr/local/cvs/auv/altex/onboard/utils/Math.cc,v 1.6 2001/06/02 21:33:53 hthomas Exp $";

/*
$Log: Math.cc,v $
Revision 1.6  2001/06/02 21:33:53  hthomas
various changes; remove stale entries in root makefile, fixed file naming to allow for serving from windows, and fixed makefiles to avoid file linking/unlinking problem in makedepend when talking through samba to windows boxes

Revision 1.5  2000/08/21 20:54:20  oreilly
Added round() method

Revision 1.4  2000/05/02 21:04:10  rob
Corrected conversion constant.

Revision 1.3  2000/05/02 21:01:02  rob
Added rpm to radians per second conversion constant.

Revision 1.2  2000/02/07 20:59:22  pean
Added revised header to include Proprietary Information

Revision 1.1  1999/12/07 23:03:05  pean
Created utils subdirectory and moved source files here to help manage sources

Revision 1.4  1999/12/03 23:21:15  rob
Moved some functions from DynamicControl here.

Revision 1.3  1999/10/12 23:17:47  oreilly
*** empty log message ***

Revision 1.2  1999/09/15 23:03:41  pean
Added kvh compass interface module

Revision 1.1  1999/09/09 18:39:38  oreilly
*** empty log message ***

Revision 1.3  1997/04/29 10:15:49  oreilly
Use more efficient algorithms for angle normalization

Revision 1.2  96/10/28  08:53:24  08:53:24  oreilly (Thomas C. O'Reilly)
Math utilities

Revision 1.1  96/08/14  13:30:38  13:30:38  oreilly (Thomas C. O'Reilly)
Initial revision

*/
#include "MathP.h"

double const Math::TwoPi      = M_PI  * 2.0;
double const Math::RadsPerDeg = M_PI  / 180.0;
double const Math::DegsPerRad = 180.0 / M_PI;
double const Math::RpmToRadps = M_PI  / 30.0;

void Math::zeroToTwoPi(Radians *angle)
{
  float m = 1.;
  if (*angle < 0.)
    m = -1.;
  
  double ratio = fabs(*angle) / TwoPi;

  *angle = m * TwoPi * (ratio - floor(ratio));

  while (*angle < 0.0)
    (*angle) += TwoPi;
  
  while (*angle > TwoPi)
    (*angle) -= TwoPi; 
}


void Math::minusPiToPi(Radians *angle)
{
  Math::zeroToTwoPi(angle);
  
  if (*angle < 0.0)
  {
    while (fabs(*angle) > M_PI)
      (*angle) += (TwoPi);
  }
  else
  {
    while (fabs(*angle) > M_PI)
      (*angle) -= (TwoPi);
  }
}


Radians Math::angularSeparation(Radians start, Radians stop)
{
  Radians delta = stop - start;
  while (delta > TwoPi)
    delta -= TwoPi;
  
  while (delta < 0.0)
    delta += TwoPi;

  return delta;
}

Radians Math::degToRad(double degs)
{
  return degs * RadsPerDeg;
}

double Math::radToDeg(Radians rads)
{
  return rads * DegsPerRad;
}


/* ==================================================================
|    LIMIT:
|      Makes sure a variable falls within defined limits...
| ================================================================== */

double Math::limit( double value, double max, double min )
{
    double dummy;

    if ( max < min )
    {
        dummy = max;
        max = min;
        min = dummy;
    }

    if ( value > max )
        value = max;
    if ( value < min )
        value = min;
    return value;
}

/*===================================================================*/

double Math::sgn( double x )
{
    if ( x < 0 )
        return -1;
    else
        return  1;
}

/* --------------------------------------------------------------------------
| modPi:
|
|   J. G. Bellingham, 3/1/94
  -------------------------------------------------------------------------- */

double Math::modPi( double angle )
{
    double ipart, result;

    result = 2*PI*modf( angle/(2*PI), &ipart );

    if ( result >= PI )
        result -= 2*PI;

    if ( result < -PI )
        result += 2*PI;

    return result;
}



int Math::round(double x)
{
  double incr;
  if (x > 0.) {
    incr = 0.5;
  }
  else {
    incr = -0.5;
  }

  return (int )(x + incr);
}