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

/////////////////////////////////////////////////////////////////////
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
//\\#define new DEBUG_NEW
#endif
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////
// CLASS CPMath 
/////////////////////////////////////////////////////////////////////
const double CPMath::PI = 3.14159265358979323846;
const double CPMath::TWOPI = PI * 2.0;
const double CPMath::DEG_TO_RAD = PI / 180.0f;
const double CPMath::RAD_TO_DEG = 180.0f / PI;

const double CPMath::MU_OVER_4PI = 0.0000001;

const double CPMath::M_TO_CM = 100.0;
const double CPMath::CM_TO_M = 0.01;

const double CPMath::IN_TO_CM = 2.54;
const double CPMath::CM_TO_IN = (1.0 / IN_TO_CM);

const double CPMath::IN_TO_M = 0.0254;
const double CPMath::M_TO_IN = (1.0/IN_TO_M);

const double CPMath::FT_TO_IN = 12.0;
const double CPMath::IN_TO_FT = (1.0 / FT_TO_IN);	// inches to feet

const double CPMath::FT_TO_CM = (FT_TO_IN * IN_TO_CM);
const double CPMath::CM_TO_FT = (1.0 / FT_TO_CM);

const double CPMath::FT_TO_M  = (FT_TO_IN * IN_TO_M);
const double CPMath::M_TO_FT  = (1.0 / FT_TO_M);

double CPMath::ScaleFactorPos[E_P_POS_MAX][E_P_POS_MAX] = {
	{ 1.0f,		IN_TO_FT,	IN_TO_CM,	IN_TO_M },
	{ FT_TO_IN, 1.0f,		FT_TO_CM,	FT_TO_M },
	{ CM_TO_IN, CM_TO_FT,	1.0f,		CM_TO_M },
	{ M_TO_IN,	M_TO_FT,	M_TO_CM,	1.0f }	};

double CPMath::ScaleFactorEuler[E_P_ORI_MAX][E_P_ORI_MAX] = {
	{ 1.0f,       DEG_TO_RAD, 0.0f },
	{ RAD_TO_DEG, 1.0f,       0.0f },
	{ 0.0f,       0.0f,       1.0f } };

// Usable AlmostEqual function

/*bool 
CPMath::fpEQ(float A, float B, int maxUlps)
{
    // Make sure maxUlps is non-negative and small enough that the

    // default NAN won't compare as equal to anything.

    if (!(maxUlps > 0 && maxUlps < 4 * 1024 * 1024))
	{ return false; }

    int aInt = *(int*)&A;
    int bInt = *(int*)&B;

    // Make aInt lexicographically ordered as a twos-complement int

    if (aInt < 0)
	{
        aInt = 0x80000000 - aInt;
	}
    // Make bInt lexicographically ordered as a twos-complement int
	if (bInt < 0)
	{
        bInt = 0x80000000 - bInt;
	}

    int intDiff = abs(aInt - bInt);
    if (intDiff <= maxUlps)
	{
        return true;
	}

    return false;
}*/

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
int32_t 
CPMath::isnan(float f) 
{
    //    3           2          1  bits...
    // 2 1098765 43 210987 65432109 876543210
    // s. exponent . significand

    const long exponentbits = 0x7F800000; // 8 bits 
    const long significandbits = 0x007FFFFF; // 23 bits
    
    // look at the memory of the float as a long
    long *ilp = (long*)&f; // ilp = incoming long pointer

    if ((exponentbits & *ilp) == exponentbits) { // all exponent bits set
        return 1;
    }
    return 0;
}

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
// sometimes we have a nan when the significand is zero!
//        if ((significandbits & *ilp) != 0) { // significand non-zero
//        }
double 
CPMath::rint(double f) 
{
    
#ifdef _WIN32    
    double doub = f;
    if ( doub >0.0)
    {
       if ( ( doub + 0.5 - (long) doub ) > 1.0)
          return (double)(int)(doub + 1.0) ;
       else
          return (double)(int)doub ;
    }
    else
    {
       if ( ( doub - 0.5 - (long) doub ) < - 1.0)
          return (double)(int)(doub - 1.0) ;
       else
          return (double)(int)doub ;
    }
#else // in IRIX... rint is part of the math library
    return rint(f);
#endif

}

uint8_t
CPMath::CalcXOR( uint8_t* pBody, uint16_t usSize )
{
	uint8_t xor = 0;

	for (int i=0; i<usSize; i++)
	{
		xor ^= pBody[i];
	}

	return xor;
}



//
// From: http://www.devx.com/tips/Tip/15024
//
//UINT32
//CPMath::count_bits_first_version(UINT32 bits)
//{
//    bits = ((bits >> 1) & 0x55555555) + (bits & 0x55555555);
//    bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333);
//    bits = ((bits >> 4) & 0x0F0F0F0F) + (bits & 0x0F0F0F0F);
//    bits = ((bits >> 8) & 0x00FF00FF) + (bits & 0x00FF00FF);
//    bits = ((bits >>16) & 0x0000FFFF) + (bits & 0x0000FFFF);
//    return bits;
//}
//
//This code can be made faster by removing unnecessary operations. 
//Here is the fully optimized version.
//
uint32_t
CPMath::count_bits(uint32_t bits)
{
    bits = bits - ((bits >> 1) & 0x55555555);
    bits = ((bits >> 2) & 0x33333333) + (bits & 0x33333333);
    bits = ((bits >> 4) + bits) & 0x0F0F0F0F;
    return (bits * 0x01010101) >> 24;
}



/////////////////////////////////////////////////////////////////////
// END $Workfile: VPmathUT.cpp $
/////////////////////////////////////////////////////////////////////
