/////////////////////////////////////////////////////////////////////
// Polhemus Inc., www.polhemus.com
// © 2002 Polhemus Inc., All Rights Reserved
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
//
//  Filename:           $Workfile: VPtypesUT.cpp $
//
//  Project Name:       Polhemus Tracker Command Interface API 
//
//  Original Author:    S. Gagnon
//
//  Description:        Custom Data Type Classes
//
//  VSS $Header: /Viper/VPCmdIF/VPCmdIF/VPtypesUT.cpp 1     9/22/17 6:00p Suzanne $  
//
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////

#include "VPtypesUT.h"

#include "VPmat3x3.h"

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

////////////////////////////////////////////////////////////////////
///// COriVec
/////////////////////////////////////////////////////////////////////
void
COriVec::FromQtrn(const COriVec & q)
{
	if (!(q.Quaternion()))
		return;

	if (Quaternion())
	{
		*this = q;
	}
	else
	{
		CDirCos dc(q);
		dc.toEuler(*this);
	}
}
void
COriVec::FromEuler(const COriVec & e)
{
	if (e.Quaternion())
		return;

	if (Quaternion())
	{
		Euler2Qtrn(e);
	}
	else
	{
		*this = e;
		Scale((float)(CPMath::ScaleFactorEuler[(uint32_t)m_eUnits][(int32_t)(e.Units())]));
	}
}
void
COriVec::Euler2Qtrn(const COriVec & e)
{
	if (e.Quaternion())
		return;				// return if rv is not Euler

	if (!(Quaternion()))	// return if lv is not Qtrn
		return;

	COriVec rv(e);
	float   sa, se, sr;
	float   ca, ce, cr;

	if (!(rv.Degrees()))
	{
		rv.Scale((float)CPMath::DEG_TO_RAD);
	}

	rv.Scale(0.5f);

	sa = (float)sin(rv.az());
	se = (float)sin(rv.el());
	sr = (float)sin(rv.ro());
	ca = (float)cos(rv.az());
	ce = (float)cos(rv.el());
	cr = (float)cos(rv.ro());

	q(0) = ca * ce * cr + sa * se * sr;
	q(1) = ca * ce * sr - sa * se * cr;
	q(2) = ca * se * cr + sa * ce * sr;
	q(3) = sa * ce * cr - ca * se * sr;

	NormalizeQtrn();

}
void
COriVec::NormalizeQtrn()
{
	if (!(Quaternion()))
		return;

	float fSos;             // sos: sum of squares
	float fNormalizer;

	fSos = at(0) * at(0) + at(1) * at(1) + at(2) * at(2) + at(3) * at(3);
	fNormalizer = 1 / (float)sqrt(fSos);

	if (at(0) < 0.0f)
	{
		fNormalizer = -fNormalizer;
	}

	at(0) *= fNormalizer;
	at(1) *= fNormalizer;
	at(2) *= fNormalizer;
	at(3) *= fNormalizer;
}
void
COriVec::InitQtrn()
{
	if (!(Quaternion()))
		return;
	Zero();
	at(0) = 1.0f;
}

void
CPiTypeBase::ToDispString( const int8_t & cVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, cVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const uint8_t & ucVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, ucVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const int16_t & sVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, sVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const uint16_t & wVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, wVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const int32_t & iVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, iVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const uint32_t & uiVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, uiVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const int64_t & llVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, llVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const uint64_t & ullVal, tstring & s, LPCTSTR szFmt )
{
	TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, ullVal);
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const float & fVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, fVal );
	s += szTmp;
}

void
CPiTypeBase::ToDispString( const double & dVal, tstring & s, LPCTSTR szFmt )
{
    TCHAR szTmp[50];

	_stprintf_s(szTmp, _countof(szTmp), szFmt, dVal );
	s += szTmp;
}

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

void
CPiTypeBase::BinToDispString( const int8_t &  cVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	BinToDispString( (uint8_t)(cVal), s, szFmt );
}

void
CPiTypeBase::BinToDispString( const uint8_t & ucVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	//for (int32_t i=0; i<8; i++)
	for ( int32_t i=7; i>=0; i-- )
	{
		ToDispString( ((ucVal >> i) & (0x01)), s, szFmt );
	}
}

void
CPiTypeBase::BinToDispString( const int16_t & sVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	//for (int32_t i=0; i<sizeof(int16_t); i++)
	for (int32_t i=sizeof(int16_t)-1; i>=0; i-- )
	{
		BinToDispString( (uint8_t)((sVal >> (i*8)) & 0xff), s, szFmt );
	}
}

void
CPiTypeBase::BinToDispString( const uint16_t &  wVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	//for (int32_t i=0; i<sizeof(uint16_t); i++)
	for (int32_t i=sizeof(uint16_t)-1; i>=0; i-- )
	{
		BinToDispString( (uint8_t)((wVal >> (i*8)) & 0xff), s, szFmt );
	}
}

void
CPiTypeBase::BinToDispString( const int32_t &   iVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	BinToDispString( (uint32_t &)(iVal), s, szFmt );
}

void
CPiTypeBase::BinToDispString( const uint32_t & uiVal, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	for (int32_t i=sizeof(uint32_t)-1; i>=0; i-- )
	{
		BinToDispString( (uint8_t)((uiVal >> (i*8)) & 0xff), s, szFmt );
	}
}

void
CPiTypeBase::BinToDispString( const int64_t & ll64Val, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/ )
{
	//for (int32_t i=0; i<sizeof(int64_t); i++)
	for (int32_t i=sizeof(int64_t)-1; i>=0; i-- )
	{
		BinToDispString( (uint8_t)((ll64Val >> (i*8)) & 0xff), s, szFmt );
	}
}

void
CPiTypeBase::BinToDispString(const uint64_t & ull64Val, tstring & s, LPCTSTR szFmt/*=PI_B_FMT*/)
{
	for (int32_t i = sizeof(uint64_t)-1; i >= 0; i--)
	{
		BinToDispString((uint8_t)((ull64Val >> (i * 8)) & 0xff), s, szFmt);
	}
}


/////////////////////////////////////////////////////////////////////
// END $Workfile: VPtypesUT.cpp $
/////////////////////////////////////////////////////////////////////
