/////////////////////////////////////////////////////////////////////
// Polhemus Inc.,  www.polhemus.com
// © 2003 Alken, Inc. dba Polhemus, All Rights Reserved
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
//
//  Filename:           $Workfile: VPmathUT.h $
//
//  Project Name:       Polhemus Tracker Command Interface API 
//
//  Original Author:    S. Gagnon
//
//  Description:        Custom Math Helper Class
//
//  VSS $Header: /Viper/VPCmdIF/VPCmdIF/VPmathUT.h 1     9/22/17 6:00p Suzanne $  
//
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
#ifndef _PMATH_H_
#define _PMATH_H_

#include <cmath>
#include <stdint.h>

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
// CLASS CPMath
/////////////////////////////////////////////////////////////////////

#define PI_FP_EPSILON 0.00001
#define PI_FLOAT_EQ(x,v) (((v - PI_FP_EPSILON) < x) && (x <( v + PI_FP_EPSILON)))

typedef enum 
{
	E_P_ORI_EULER_DEGREE = 0 /**< Euler angles in degrees. */
	, E_P_ORI_EULER_RADIAN   /**< Euler angles in radians. */
	, E_P_ORI_QUATERNION     /**< Quaternions. */

	, E_P_ORI_MAX
} ePOriUnits;


typedef enum 
{
	E_P_POS_INCH = 0 /**< Inches. */
	, E_P_POS_FOOT   /**< Feet. */
	, E_P_POS_CM     /**< Centimeters. */
	, E_P_POS_METER  /**< Meters. */

	, E_P_POS_MAX
} ePPosUnits;

class CPMath 
{

public:
	static const double PI;
	static const double TWOPI;
	static const double DEG_TO_RAD;
	static const double RAD_TO_DEG;

	static const double	MU_OVER_4PI;

	static const double M_TO_CM;	// meters to cm
	static const double CM_TO_M;
	static const double IN_TO_CM;
	static const double CM_TO_IN;
	static const double IN_TO_M;
	static const double M_TO_IN;
	static const double FT_TO_IN;
	static const double IN_TO_FT;
	static const double FT_TO_CM;
	static const double CM_TO_FT;
	static const double FT_TO_M;
	static const double M_TO_FT;

	static double ScaleFactorPos[E_P_POS_MAX][E_P_POS_MAX];
	static double ScaleFactorEuler[E_P_ORI_MAX][E_P_ORI_MAX];

    static float degToRad (float angleDeg) {
        return (float)angleDeg * (float)DEG_TO_RAD;
    }
    static float radToDeg (float angleRad) {
        return (float)angleRad * (float)RAD_TO_DEG;
    }

	static double SQR ( double n ) { return pow( n, 2 ); }

    // is this float a nan???
    static int isnan(float f);

    // return the integer nearest f, rounded
    static double rint(double f);


	static bool fpEQ( float a, float b ){ return PI_FLOAT_EQ(a,b); }

	static bool fpEQ(int32_t na, int32_t nb){ return fpEQ((float)na, (float)nb); }

	static uint8_t CalcXOR( uint8_t * pBody, uint16_t usSize );

	static uint32_t count_bits(uint32_t bits);

	template< typename T>
	static T Clamp( const T& x, const T& low, const T& high ) {
		return x < low ? low : (x > high ? high : x);
	}



};
/////////////////////////////////////////////////////////////////////
// END $Workfile: VPmathUT.h $
/////////////////////////////////////////////////////////////////////
#endif // _PIMATH_H_
