#pragma once

#include <math.h>												// pow
#include <stdio.h>											// sprintf

#ifndef PI
	#define PI 3.14159265358979323846
#endif

const DWORD g_dwCMCalcSideLength=(DWORD)pow((double)(0x7fffff/6/4),1.0f/3.0);
const DWORD g_dwCMSideLength=((g_dwCMCalcSideLength-1)/2)*2+1;										// to make sure vector like (1,0,0),(0,-1,0) are prefectly reproduced
const DWORD g_dwCMCalcAngleLength=0x7fffff/(g_dwCMSideLength*6*g_dwCMSideLength);
const DWORD g_dwCMAngleLength=(g_dwCMCalcAngleLength/4)*4;												// to make sure angles like 0,90,180,270 are prefectly reproduced
const DWORD g_dwCMVectorMaxColor=g_dwCMSideLength*g_dwCMSideLength*6;							// 6 for dominant direction
const DWORD g_dwCMBaseMaxColor=2*g_dwCMCalcAngleLength*g_dwCMVectorMaxColor;			// 2 for parity
const DWORD g_dwCMStartBaseColor=0x7fffff-g_dwCMBaseMaxColor/2;


// stores a orthogonal base (3 3d unit length vectors, perpendicular to each other) in 3 Bytes (24Bit Color)
//
class CRGBPackedBase
{
public:

	//! 
	DWORD GetRGB( void ) const
	{
		return(m_dwData);
	}

	//! 
	void SetRGB( DWORD indwValue )
	{
		m_dwData=indwValue;
	}

	//! compress the base vectors
	//! /param inV1 input vector (has to be normalized)
	//! /param inV2 input vector (has to be normalized)
	//! /param inV3 input vector (has to be normalized)
	void SetBase( const float inV1[3], const float inV2[3], const float inV3[3] )
	{
		bool bParity=CalcParity(inV1,inV2,inV3);
		m_dwData=CalcColorFromBase(inV1,inV2,bParity);
	}

	//! uncompress the base vectors
	//! /param outV1
	//! /param outV2
	//! /param outV3
	bool GetBase( float outV1[3], float outV2[3], float outV3[3] ) const
	{
		if(!m_dwData)return(false);

		bool bParity;
		CalcBaseFromColor(m_dwData,outV1,outV2,bParity);

		CrossProd(outV3,outV1,outV2);
		if(!bParity)
		{
			outV3[0]=-outV3[0];
			outV3[1]=-outV3[1];
			outV3[2]=-outV3[2];
		}
		return(true);
	}

	//! 
	void SetVec( const float inV1[3] )
	{
		m_dwData=CalcColorFromVec(inV1);
	}

	//! 
	DWORD GetVec( float outV[3] ) const
	{
		return(CalcColorFromVec(outV));
	}

	// *******************************************************************************

private:


	DWORD	m_dwData;						//!< RGB compressed base in 0x00ffffff, 0=invalid 




	// *******************************************************************************

	static void CalcVecFromColor( const DWORD indwColor, float outVec[3] )
	{
		int iDominantDirection=indwColor%6;

		DWORD dwIntermediate=indwColor/6;
		DWORD dwX=dwIntermediate%g_dwCMSideLength;
		DWORD dwY=dwIntermediate/g_dwCMSideLength;

		float fX=(((float)dwX+0.5f)/(float)(g_dwCMSideLength))*2.0f-1.0f;
		float fY=(((float)dwY+0.5f)/(float)(g_dwCMSideLength))*2.0f-1.0f;

		switch(iDominantDirection)
		{
			case 0:	outVec[0]=1.0f;		outVec[1]=fX;			outVec[2]=fY;			break;
			case 1: outVec[0]=fY;			outVec[1]=1.0f;		outVec[2]=fX;			break;
			case 2: outVec[0]=fX;			outVec[1]=fY;			outVec[2]=1.0f;		break;
			case 3: outVec[0]=-1.0f;	outVec[1]=fX;			outVec[2]=fY;			break;
			case 4: outVec[0]=fY;			outVec[1]=-1.0f;	outVec[2]=fX;			break;
			case 5: outVec[0]=fX;			outVec[1]=fY;			outVec[2]=-1.0f;	break;
			default: assert(0);
		}

		Normalize(outVec);

	/*
		char str[256];
		sprintf(str,"CalcVecFromColor %p (%.2f %.2f %.2f) %.2f %.2f %d %d\n",indwColor,outVec[0],outVec[0],outVec[0],
									fX,fY,dwX,dwY);
		OutputDebugString(str);
	*/
	}




	static DWORD CalcColorFromBase( const float inV1[3], const float inV2[3], bool inbParity )
	{
		DWORD dwPart1=CalcColorFromVec(inV1);

		float vDirection[3],vVecA[3],vVecB[3];

		CalcVecFromColor(dwPart1,vDirection);

		GetOtherBaseVec(vDirection,vVecA,vVecB);

		float fX=ScalarProd(vVecA,inV2);
		float fY=ScalarProd(vVecB,inV2);

		float fAngle=(float)atan2(fY,fX);																					// -PI .. PI
	/*
		char str[256];
		sprintf(str,"CalcColorFromBase %.2f (%.2f %.2f)\n",fAngle,fX,fY);
		OutputDebugString(str);
	*/
		DWORD dwAngle=(DWORD)(((fAngle+PI)/(PI*2.0f))*(float)g_dwCMAngleLength);		// 0 .. g_dwCMAngleLength-1

		dwAngle%=g_dwCMAngleLength;

		return(dwPart1+dwAngle*g_dwCMVectorMaxColor+(inbParity?0x800000:0));
	}



	static void CalcBaseFromColor( const DWORD dwColor, float outV1[3], float outV2[3], bool &outbParity )
	{
		float vVecA[3],vVecB[3];
		DWORD dwBase=dwColor&0x7fffff;

		outbParity=((dwColor&0x800000)!=0);
		CalcVecFromColor(dwBase%g_dwCMVectorMaxColor,outV1);

		GetOtherBaseVec(outV1,vVecA,vVecB);

		DWORD dwAngle=dwBase/g_dwCMVectorMaxColor;

		float fAngle=(float)(  ((double)(dwAngle)/(double)g_dwCMAngleLength)*(PI*2.0f)-PI   );
	/*
		char str[256];
		sprintf(str,"CalcBaseFromColor %.2f (%.2f %.2f)\n",fAngle,(float)cos(fAngle),(float)sin(fAngle));
		OutputDebugString(str);
	*/
		float c=cry_cosf(fAngle);
		float s=cry_sinf(fAngle);

		// outV2=vVecA*c+vVecB*s;
		outV2[0]=vVecA[0]*c+vVecB[0]*s;
		outV2[1]=vVecA[1]*c+vVecB[1]*s;
		outV2[2]=vVecA[2]*c+vVecB[2]*s;
	}


		
	//
	static DWORD CalcColorFromVec( const float inV[3] )
	{
		int iDominantDirection=-1;

		float vAbs[3]={ inV[0], inV[1], inV[2] };

		// calc absolute value
		if(vAbs[0]<0)vAbs[0]=-vAbs[0];
		if(vAbs[1]<0)vAbs[1]=-vAbs[1];
		if(vAbs[2]<0)vAbs[2]=-vAbs[2];

		if(vAbs[0]>=vAbs[1] && vAbs[0]>=vAbs[2])iDominantDirection=0;
		else
		if(vAbs[1]>=vAbs[0] && vAbs[1]>=vAbs[2])iDominantDirection=1;
		else
		if(vAbs[2]>=vAbs[1] && vAbs[2]>=vAbs[0])iDominantDirection=2;

		assert(iDominantDirection!=-1 && "this should never hapen or the vector has zero length");
		if(iDominantDirection==-1)iDominantDirection=0;

		float fX=inV[(iDominantDirection+1)%3];
		float fY=inV[(iDominantDirection+2)%3];
		float fZ=inV[iDominantDirection];

		if(vAbs[iDominantDirection]!=inV[iDominantDirection])iDominantDirection+=3;

		float fScale=1.0f/(float)fabs(fZ);

		fX*=fScale;fY*=fScale;

		DWORD dwX=(DWORD)((fX*0.5f+0.5f)*g_dwCMSideLength);
		DWORD dwY=(DWORD)((fY*0.5f+0.5f)*g_dwCMSideLength);

		if(dwX>=g_dwCMSideLength)dwX=g_dwCMSideLength-1;
		if(dwY>=g_dwCMSideLength)dwY=g_dwCMSideLength-1;

		DWORD dwResult=iDominantDirection+6*(dwX+g_dwCMSideLength*dwY);
	
	/*
		char str[256];
		sprintf(str,"CalcColorFromVec (%.2f %.2f %.2f) %.2f %.2f %d %d %p dom:%d\n",inV[0],inV[1],inV[2],
								fX,fY,dwX,dwY,dwResult,iDominantDirection);
		OutputDebugString(str);
	*/
		return(dwResult);
	}

public:
	//! precise scalar product calculation
	//! /param inA first operand
	//! /param inB second operand
	//! /return result of the calculation
	static float inline ScalarProd( const float inA[3], const float inB[3] )
	{
		return((float)((double)inA[0]*(double)inB[0]
									+(double)inA[1]*(double)inB[1]
									+(double)inA[2]*(double)inB[2]));
	}

	//! precise cross product calculation
	//! /param outDest result of the operation
	//! /param inA first operand
	//! /param inB second operand
	static void inline CrossProd( float outDest[3], const float inA[3], const float inB[3] )
	{
		assert(outDest!=inA);
		assert(outDest!=inB);

		outDest[0]=(float)((double)inA[1]*(double)inB[2]-(double)inA[2]*(double)inB[1]);
		outDest[1]=(float)((double)inA[2]*(double)inB[0]-(double)inA[0]*(double)inB[2]);
		outDest[2]=(float)((double)inA[0]*(double)inB[1]-(double)inA[1]*(double)inB[0]);
	}


	//! left or righthanded matrix
	static bool CalcParity( const float inV1[3], const float inV2[3], const float inV3[3] )
	{
		float vV12[3];

		CrossProd(vV12,inV1,inV2);

		return(ScalarProd(vV12,inV3)>0);
	}

	// precise
	static inline Normalize( float inoutValue[3] )
	{
		// Normalize result
		double fInvLength=1.0/sqrt((double)inoutValue[0]*(double)inoutValue[0]
														  +(double)inoutValue[1]*(double)inoutValue[1]
														  +(double)inoutValue[2]*(double)inoutValue[2]);

		inoutValue[0]=(float)(inoutValue[0]*fInvLength);
		inoutValue[1]=(float)(inoutValue[1]*fInvLength);
		inoutValue[2]=(float)(inoutValue[2]*fInvLength);
	}

	//! calculate 2 vector that form a orthogonal base with a given input vector (by M.M.)
	//! /param invDirection input direction (has to be normalized)
	//! /param outvA first output vector that is perpendicular to the input direction
	//! /param outvB second output vector that is perpendicular the input vector and the first output vector
	static inline void GetOtherBaseVec( const float inDirection[3], float outA[3], float outB[3] )
	{
		if(inDirection[2]<-0.5f || inDirection[2]>0.5f)
		{
			outA[0]=inDirection[2];
			outA[1]=inDirection[1];
			outA[2]=-inDirection[0];
		}
		else
		{
			outA[0]=inDirection[1];
			outA[1]=-inDirection[0];
			outA[2]=inDirection[2];
		}
		CrossProd(outB,inDirection,outA);
		CrossProd(outA,inDirection,outB);

		// without this normalization the results are not good enouth (Croos product introduce a errors)
		Normalize(outA);
		Normalize(outB);
	}

	//! very simple, could be done in a better way
	static void Orthogonalize( float inoutBaseA[3], float inoutBaseB[3], float inoutBaseC[3] )
	{

		bool bParity=CalcParity(inoutBaseA,inoutBaseB,inoutBaseC);

		CrossProd(inoutBaseC,inoutBaseA,inoutBaseB);

		Normalize(inoutBaseC);
		Normalize(inoutBaseA);

		CrossProd(inoutBaseB,inoutBaseC,inoutBaseA);
		
		if(!bParity)
		{
			inoutBaseC[0]=-inoutBaseC[0];
			inoutBaseC[1]=-inoutBaseC[1];
			inoutBaseC[2]=-inoutBaseC[2];
		}

		Normalize(inoutBaseB);

		assert(CalcParity(inoutBaseA,inoutBaseB,inoutBaseC)==bParity);
	}

};
