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

#include "VPmathUT.h"
#include <sstream>
#include <string>
#include <stdint.h>
#include <vector>
#include <algorithm>

#if defined (PTYPES_EXPORTS)
#define PTYPES_API __declspec(dllexport)
#else
#define PTYPES_API
//#define PTYPES_API __declspec(dllimport)
#endif


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

#define PI_8F3_FMT	_T("% 8.3f")
#define PI_8F4_FMT	_T("% 8.4f")
#define PI_7F3_FMT	_T("% 7.3f")
#define PI_F_FMT	PI_8F3_FMT
#define PI_10F3_FMT	_T("% 10.3f")
#define PI_10F4_FMT	_T("% 10.4f")
#define PI_G_FMT	_T("% 9.3G")
#define PI_95G_FMT	_T("% 9.5G")
#define PI_E_FMT	_T("% 13.6E")
#define PI_P_FMT	_T("% 9.6f")  

#define PI_D_FMT	_T("%d")  

#define PI_2D_FMT	_T("%2d")	// allows for possible sign
#define PI_3D_FMT	_T("%3d")
#define PI_4D_FMT	_T("%4d")
#define PI_5D_FMT	_T("%5d")
#define PI_6D_FMT	_T("%6d")
#define PI_10D_FMT	_T("%10d")
#define PI_20D_FMT	_T("%20d")

#define PI_02D_FMT	_T("%02d")
#define PI_03D_FMT	_T("%03d")
#define PI_04D_FMT	_T("%04d")
#define PI_05D_FMT	_T("%05d")
#define PI_06D_FMT	_T("%06d")
#define PI_010D_FMT	_T("%010d")
#define PI_020D_FMT	_T("%020d")

#define PI_SIGN3D_FMT _T("%-3d")

#define PI_X_FMT	_T("0x%x")
#define PI_2X_FMT	_T("0x%2x")
#define PI_4X_FMT	_T("0x%4x")
#define PI_8X_FMT	_T("0x%8x")
#define PI_16X_FMT	_T("0x%16x")

#define PI_02X_FMT	_T("0x%02x")
#define PI_04X_FMT	_T("0x%04x")
#define PI_08X_FMT	_T("0x%08x")
#define PI_016X_FMT	_T("0x%016x")

#define PI_H_FMT	_T("%x")
#define PI_2H_FMT	_T("%2x")
#define PI_4H_FMT	_T("%4x")
#define PI_8H_FMT	_T("%8x")
#define PI_16H_FMT	_T("%16x")

#define PI_02H_FMT	_T("%02x")
#define PI_04H_FMT	_T("%04x")
#define PI_08H_FMT	_T("%08x")
#define PI_016H_FMT	_T("%016x")

#define PI_B_FMT	_T("% 1d")

#define PI_SPACE	_T(' ')
#define PI_COMMA	_T(',')
#define PI_CRLF		_T("\r\n")
#define PI_2SPACE	_T("  ")
#define PI_4SPACE	_T("    ")
#define PI_6SPACE	_T("      ")
#define PI_8SPACE	_T("        ")
#define PI_NEWLINE	_T("\n")

/////////////////////////////////////////////////////////////////////
#define PI_uint32_t_RAW_LEN	4
#define PI_LONG_RAW_LEN		4
#define PI_int32_t_RAW_LEN	4
#define PI_int16_t_RAW_LEN	2
#define PI_FLOAT_RAW_LEN	4
#define PI_BYTE_RAW_LEN		1
#define PI_CHAR_RAW_LEN		1
/////////////////////////////////////////////////////////////////////
// CLASS CPiTypeBase
/////////////////////////////////////////////////////////////////////
class CPiTypeBase
{
public:
 	static void ToDispString( const int8_t &    cVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void ToDispString( const uint8_t &   ucVal,		 tstring & s, LPCTSTR szFmt=PI_X_FMT );
	static void ToDispString( const int16_t &   sVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void ToDispString( const uint16_t &  wVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void ToDispString( const int32_t &   iVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void ToDispString( const uint32_t &  uiVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void ToDispString( const int64_t &   ll64Val,     tstring & s, LPCTSTR szFmt=PI_X_FMT );
	static void ToDispString( const uint64_t &  ull64Val,	 tstring & s, LPCTSTR szFmt=PI_X_FMT);
	static void ToDispString( const float &     fVal,		 tstring & s, LPCTSTR szFmt=PI_F_FMT );
	static void	ToDispString( const double &    dVal,		 tstring & s, LPCTSTR szFmt=PI_G_FMT );

	static void BinToDispString( const int8_t &    cVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const uint8_t &   ucVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const int16_t &   sVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const uint16_t &  wVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const int32_t &   iVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const uint32_t &  uiVal,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const int64_t &   ll64Val,		 tstring & s, LPCTSTR szFmt=PI_D_FMT );
	static void BinToDispString( const uint64_t &  ull64Val,	 tstring & s, LPCTSTR szFmt=PI_D_FMT);
};


class PTYPES_API CVecType : public std::vector<float>
{
public:

	CVecType(int32_t m, float f)
		: std::vector<float>(m, f)
	{};

	CVecType(const CVecType & rv)
	{
		*this = rv;
	}

	operator float *		() // const pointer to 3 floats
							{ return data(); }

	CVecType & operator*=	(float f)
							{ Scale(f); return *this; }
	size_t   Size(void)
	{
		return size() * sizeof(float);
	}

	void Scale(float sf)
	{
		std::for_each(begin(), end(), [&](float &el)
		{ 
			el *= sf; 
		});
	}

	void Zero()
	{
		memset(data(), 0, size() * sizeof(float));
	}

	void Export(float * pDest)
	{
		if (!pDest)
			return;

		memcpy(pDest, data(), size() * sizeof(float));
	}

	void ToDispString(tstring & s, LPCTSTR szFmt = PI_F_FMT, TCHAR sep = PI_SPACE) const
	{
		TCHAR szTmp[50];
		//iterate through vector and print each value
		std::for_each(begin(), end(), [&s, szFmt, sep, &szTmp](float f){
			_stprintf_s((LPTSTR)szTmp, _countof(szTmp), szFmt, f);
			s += (LPCTSTR)szTmp;
			s += (TCHAR)sep; });
	}
};

class PTYPES_API C3Vec : public CVecType
{
public:
	C3Vec() : CVecType(3, 0.0f){};
	C3Vec(float x, float y, float z) : CVecType(3, 0.0f)
	{
		at(0) = x; at(1) = y; at(2) = z;
	}
	C3Vec(float * pv) : CVecType(3, 0.0f)
	{
		memcpy(data(), pv, size() * sizeof(float));
	}

	C3Vec(const C3Vec & rv) : CVecType(3, 0.0f)
	{
		*this = rv;
	}
	C3Vec & operator=	(const float r[3])
						{ at(0)  = r[0]; at(1)  = r[1]; at(2)  = r[2]; return *this; }
	C3Vec & operator+=	(const C3Vec & rv)
						{ at(0)+=rv[0]; at(1)+=rv[1]; at(2)+=rv[2]; return *this; }
	C3Vec & operator-=	(const C3Vec & rv)
						{ at(0) -= rv[0]; at(1) -= rv[1]; at(2) -= rv[2]; return *this; }

	float operator *	(const C3Vec & rv) const  // scalar (dot) product
						{ return at(0)*rv[0] + at(1)*rv[1] + at(2)*rv[2]; }

	static size_t Size(){ return 3 * sizeof(float); }
};

class PTYPES_API CPosVec : public CVecType
{
public:
	ePPosUnits m_eUnits;

	CPosVec(uint32_t U = E_P_POS_INCH)
		: CVecType(3, 0.0f)
		, m_eUnits((ePPosUnits)U)
	{};

	CPosVec(float *pf, uint32_t U = E_P_POS_INCH)
		: CVecType(3, 0.0f)
		, m_eUnits((ePPosUnits)U)
	{
		memcpy(data(), pf, size() * sizeof(float));
	}

	CPosVec(const CPosVec & rv)
		: CVecType( 3, 0.0f )
	{
		*this = rv;
		m_eUnits = rv.Units();
	}

	void Report(tstring & s, LPCTSTR szFmt = PI_F_FMT, TCHAR sep = PI_SPACE) const
	{
		ToDispString(s);
		switch (m_eUnits)
		{
		case E_P_POS_INCH:// = 0 /**< Inches. */
			s += _T("E_P_POS_INCH\r\n");
			break;
		case E_P_POS_FOOT:   /**< Feet. */
			s += _T("E_P_POS_FOOT\r\n");
			break;
		case E_P_POS_CM:     /**< Centimeters. */
			s += _T("E_P_POS_CM\r\n");
			break;
		case E_P_POS_METER:  /**< Meters. */
			s += _T("E_P_POS_METER\r\n");
			break;
		}
	}

	void Convert(ePPosUnits uNew)
	{
		Scale((float)CPMath::ScaleFactorPos[m_eUnits][uNew]);
		m_eUnits = uNew;
	}

	void ConvertTo(float *pDest, ePPosUnits uDest)
	{
		if (!pDest)
			return;

		CPosVec vecDest(*this);

		vecDest.Convert(uDest);
		vecDest.Export(pDest);
	}

	ePPosUnits Units() const	    { return m_eUnits; }
	float & x()						{ return at(0);    }
	float & y()						{ return at(1);    }
	float & z()						{ return at(2);    }

	const float & x() const			{ return at(0); }
	const float & y() const			{ return at(1); }
	const float & z() const			{ return at(2); }
};

class PTYPES_API COriVec : public std::vector<float>
{
public:
	ePOriUnits m_eUnits;

	COriVec(uint32_t U = E_P_ORI_QUATERNION) 
		: std::vector<float>((U == E_P_ORI_QUATERNION) ? 4 : 3, 0.0f)
		, m_eUnits((ePOriUnits)U)
		{
			InitQtrn();
		};

	COriVec(float *pf, uint32_t U = E_P_ORI_QUATERNION)
		: std::vector<float>((U == E_P_ORI_QUATERNION) ? 4 : 3, 0.0f)
		, m_eUnits((ePOriUnits)U)
	{
		memcpy(data(), pf, size() * sizeof(float));
	}

	// copy constructor
	COriVec(const COriVec & rv)
	{
		*this = rv;
		m_eUnits = rv.Units();
	}

	void ToDispString(tstring & s, LPCTSTR szFmt = PI_F_FMT, TCHAR sep = PI_SPACE) const
	{
		TCHAR szTmp[50];
		//iterate through vector and print each value
		std::for_each(begin(), end(), [&s, szFmt, sep, &szTmp](float f){
			_stprintf_s((LPTSTR)szTmp, _countof(szTmp), szFmt, f);
			s += (LPCTSTR)szTmp;
			s += (TCHAR)sep; });
	}

	void Report(tstring & s, LPCTSTR szFmt = PI_F_FMT, TCHAR sep = PI_SPACE) const
	{
		ToDispString(s);
		switch (m_eUnits)
		{
		case E_P_ORI_EULER_DEGREE:
			s += _T("E_P_ORI_EULER_DEGREE\r\n");
			break;
		case E_P_ORI_EULER_RADIAN:
			s += _T("E_P_ORI_EULER_RADIAN\r\n");
			break;
		case E_P_ORI_QUATERNION:
			s += _T("E_P_ORI_QUATERNION\r\n");
			break;
		}
	}
	// convert this to array of floats at pDest in units uDest
	void ConvertTo(float *pDest, ePOriUnits uDest)
	{
		if (!pDest)
			return;

		COriVec vecDest(uDest);

		if (Quaternion())
			vecDest.FromQtrn(*this);
		else
			vecDest.FromEuler(*this);

		memcpy(pDest, vecDest.data(), vecDest.size() * sizeof(float));
	}
	
	//Converts r-value to this units and copies into this
	void ConvertFrom(const COriVec & rv)
	{
		if (rv.Quaternion())
			FromQtrn(rv);

		else
			FromEuler(rv);
	}

	void Scale(float sf)
	{
		std::for_each(begin(), end(), [&sf](float el){ el *= sf; });
	}

	ePOriUnits Units() const			    { return m_eUnits; }
	bool Quaternion() const					{ return m_eUnits == E_P_ORI_QUATERNION; }
	bool Degrees() const					{ return m_eUnits == E_P_ORI_EULER_DEGREE; }
	void FromQtrn(const COriVec & q);
	void FromEuler(const COriVec & e);

	float & az()						{ return at(0); }
	float & el()						{ return at(1); }
	float & ro()						{ return at(2); }
	float & q( int32_t i )				{ return at(i); }

	const float & az() const			{ return at(0); }
	const float & el() const			{ return at(1); }
	const float & ro() const			{ return at(2); }
	const float & q(int32_t i) const	{ return at(i); }

	void NormalizeQtrn();
	void InitQtrn();
	void Zero()
	{
		memset(data(), 0, size() * sizeof(float));
	}

private:
	void Euler2Qtrn(const COriVec & e);

};


/////////////////////////////////////////////////////////////////////
// END $Workfile: VPtypesUT.h $
/////////////////////////////////////////////////////////////////////
#endif // _PITYPES_H_
