#include "math.h"
#include "logger.h"

extern Logger g_Logger;

int rMath::_3dnow;
int rMath::sse;
int rMath::mmx;
char rMath::Vendor[13];

rMath* InitMath()
{
	rMath *m;
	rMath::DetectCPU();
	if(rMath::_3dnow)
	{
		m = new r3DNow;
		return m;
	}
	
	m = new rMath;
	return m;
}

rMath::rMath()
{
	mmx = 0;
	sse = 0;
	_3dnow = 0;
	Vendor[0] = 0;
}

void rMath::MatrixMultiply(float* d, float *s1, float *s2)
{
	d[ 0] = s1[ 0] * s2[ 0] + s1[ 1] * s2[ 4] + s1[ 2] * s2[ 8] + s1[ 3] * s2[12];
	d[ 1] = s1[ 0] * s2[ 1] + s1[ 1] * s2[ 5] + s1[ 2] * s2[ 9] + s1[ 3] * s2[13];
	d[ 2] = s1[ 0] * s2[ 2] + s1[ 1] * s2[ 6] + s1[ 2] * s2[10] + s1[ 3] * s2[14];
	d[ 3] = s1[ 0] * s2[ 3] + s1[ 1] * s2[ 7] + s1[ 2] * s2[11] + s1[ 3] * s2[15];

	d[ 4] = s1[ 4] * s2[ 0] + s1[ 5] * s2[ 4] + s1[ 6] * s2[ 8] + s1[ 7] * s2[12];
	d[ 5] = s1[ 4] * s2[ 1] + s1[ 5] * s2[ 5] + s1[ 6] * s2[ 9] + s1[ 7] * s2[13];
	d[ 6] = s1[ 4] * s2[ 2] + s1[ 5] * s2[ 6] + s1[ 6] * s2[10] + s1[ 7] * s2[14];
	d[ 7] = s1[ 4] * s2[ 3] + s1[ 5] * s2[ 7] + s1[ 6] * s2[11] + s1[ 7] * s2[15];

	d[ 8] = s1[ 8] * s2[ 0] + s1[ 9] * s2[ 4] + s1[10] * s2[ 8] + s1[11] * s2[12];
	d[ 9] = s1[ 8] * s2[ 1] + s1[ 9] * s2[ 5] + s1[10] * s2[ 9] + s1[11] * s2[13];
	d[10] = s1[ 8] * s2[ 2] + s1[ 9] * s2[ 6] + s1[10] * s2[10] + s1[11] * s2[14];
	d[11] = s1[ 8] * s2[ 3] + s1[ 9] * s2[ 7] + s1[10] * s2[11] + s1[11] * s2[15];

	d[12] = s1[12] * s2[ 0] + s1[13] * s2[ 4] + s1[14] * s2[ 8] + s1[15] * s2[12];
	d[13] = s1[12] * s2[ 1] + s1[13] * s2[ 5] + s1[14] * s2[ 9] + s1[15] * s2[13];
	d[14] = s1[12] * s2[ 2] + s1[13] * s2[ 6] + s1[14] * s2[10] + s1[15] * s2[14];
	d[15] = s1[12] * s2[ 3] + s1[13] * s2[ 7] + s1[14] * s2[11] + s1[15] * s2[15];
}

void rMath::DetectCPU()
{
	int signature = 0;
	__asm
	{
		xor eax, eax
		cpuid
		mov dword ptr [Vendor], ebx
		mov dword ptr [Vendor+4], edx
		mov dword ptr [Vendor+8], ecx
		mov eax, 1
		cpuid
		mov signature, edx
		mov eax, 0x80000001
		cpuid
		mov _3dnow, edx
		and _3dnow, CPUID_3DNOW
	}
    
    mmx = signature & CPUID_MMX;
    sse = signature & CPUID_SSE;
    
    Vendor[12] = 0;

	g_Logger.Log("CPU found: %s\n\t3DNow: %s\n\tSSE: %s\n\tMMX: %s",
		Vendor,
		_3dnow ? "yes":"no",
		sse ? "yes":"no",
		mmx ? "yes":"no");
}

inline void rMath::Dot(float* d, float *s1, float *s2)
{
	*d = s1[0]*s2[0] + s1[1]*s2[1] + s1[2]*s2[2] + s1[3]*s2[3];
}

inline void r3DNow::Dot(float* d, float *s1, float *s2)
{
	__asm
	{
		mov eax, [s1]
		mov ebx, [s2]
		mov ecx, [d]

		movq mm0, [eax]
		pfmul mm0, [ebx]
		movq mm1, [eax+8]
		pfmul mm1, [ebx+8]
		pfadd mm0, mm1
		pfacc mm0, mm0
	
		movd [ecx], mm0
		femms
	}
}

inline void rMath::MemCopy(void *d, const void *s, int Bcount)
{
	memcpy(d, s, Bcount);
}

inline void r3DNow::MemCopy(void *d, const void *s1, int Bcount)
{
	void *s = (void*)s1;	//We need to be able to change this

	int count = ((int)d) & 7;	//Align to 64B
	if(count)
	{
		memcpy(d, s, count);
		__asm
		{
			mov eax, [count]
			add [d], eax
			add [s], eax
			sub [Bcount], eax
		}
	}
	__asm
	{
		mov ebx, 64
		mov edx, 0
		mov eax, [Bcount]
		div ebx

		mov [count], edx	//Store remainder
		cmp eax, 0
		jz $end

		mov esi, [s]
		mov edi, [d]
	$loop:
		prefetchnta [esi + 64]
		movq mm0, [esi+0]
		movq mm1, [esi+8]
		movq mm2, [esi+16]
		movq mm3, [esi+24]
		movq mm4, [esi+32]
		movq mm5, [esi+40]
		movq mm6, [esi+48]
		movq mm7, [esi+56]


		movntq [edi+0], mm0
		movntq [edi+8], mm1
		movntq [edi+16], mm2
		movntq [edi+24], mm3
		movntq [edi+32], mm4
		movntq [edi+40], mm5
		movntq [edi+48], mm6
		movntq [edi+56], mm7

		add esi, 64
		add edi, 64
		dec eax
		jnz $loop
		mov [s], esi
		mov [d], edi
		femms
	$end:
	}
	if(count)
		memcpy(d,s,count);	//Finish the remaining
}
		
inline float rMath::MagnitudeVA3(float *x)
{
	return (float)sqrt((*x) * (*x)
		+ (*(x+1)) * (*(x+1))
		+ (*(x+2)) * (*(x+2)));
}

inline float r3DNow::MagnitudeVA3(float *x)
{
	float mag;
	__asm
	{
		mov eax, [x]
		movq mm0, [eax]
		movd mm1, [eax+8]
		pfmul mm0, mm0
		pfmul mm1, mm1
		pfacc mm0, mm1
		pfacc mm0, mm0

	//Square root

		pfrsqrt mm1, mm0 
		movq mm2, mm1
		pfmul mm1, mm1
		punpckldq mm0, mm0
		pfrsqit1 mm1, mm0 
		pfrcpit2 mm1, mm2
		pfmul mm0, mm1

		movd [mag], mm0
		femms
	}

	return mag;
}


//TODO:  Add to math classes, these functions are taken from gametutorials.com 
float DotProduct ( vector3 &vVector1, vector3 &vVector2)
{
	return vVector1.x*vVector2.x + vVector1.y*vVector2.y + vVector1.z*vVector2.z;
}

float Absolute(float num)
{
	if(num < 0)
		return (0 - num);

	return num;
}


												
vector3 Cross(vector3 vVector1, vector3 vVector2)
{
	vector3 vNormal;					

	vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
														
	vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
														
	vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));

	return vNormal;						
}



float Magnitude(vector3 vNormal)
{

	return (float)sqrt( (vNormal.x * vNormal.x) + (vNormal.y * vNormal.y) + (vNormal.z * vNormal.z) );
}



vector3 Normalize(vector3 vNormal)
{
	float magnitude = Magnitude(vNormal);


	vNormal.x /= magnitude;			
	vNormal.y /= magnitude;					
	vNormal.z /= magnitude;			


	return vNormal;		
}



vector3 Normal(vector3 vPolygon[])					
{													
	vector3 vVector1 = vPolygon[2] - vPolygon[0];
	vector3 vVector2 = vPolygon[1] - vPolygon[0];

	vector3 vNormal = Cross(vVector1, vVector2);


	vNormal = Normalize(vNormal);

	return vNormal;
}

float Distance(vector3 vPoint1, vector3 vPoint2)
{
	double distance = sqrt( (vPoint2.x - vPoint1.x) * (vPoint2.x - vPoint1.x) +
						    (vPoint2.y - vPoint1.y) * (vPoint2.y - vPoint1.y) +
						    (vPoint2.z - vPoint1.z) * (vPoint2.z - vPoint1.z) );

	return (float)distance;
}