#include "dllinterface.h"
#include "DXMatrix.h"

float DXMatrix::Norm(void) const
{
	return 16.0f * _max(_max(fabs(_11), _max(fabs(_12), _max(fabs(_13), fabs(_14)))), _max(_max(fabs(_21), _max(fabs(_22), _max(fabs(_23), fabs(_24)))), _max(_max(fabs(_31), _max(fabs(_32), _max(fabs(_33), fabs(_34)))), _max(fabs(_41), _max(fabs(_42), _max(fabs(_43), fabs(_44)))))));
}

DXMatrix DXMatrix::Sqrt(void) const
{
	//compute X = A^(1/2)
	static const double e = 1e-6f;
	static const int maxiter = 10;

	DXMatrix X = *this;
	DXMatrix Y = IDENTITY();
	
	int i = 0; //limit maximum iterations to avoid infinite looping
	while ((X*X - *this).Norm() > e && i++ < maxiter)
	{
		float test = (X*X - *this).Norm();

		DXMatrix iX = InvertMatrix(X);
		DXMatrix iY = InvertMatrix(Y);
		X = 0.5f * (X + iY);
		Y = 0.5f * (Y + iX);
	}
	return X;
}	

DXMatrix DXMatrix::Power(const int exp) const
{
	DXMatrix C = IDENTITY();
	if (exp == 0)
		return C;

	DXMatrix A = exp < 0 ? InvertMatrix(*this) : *this;

	int b = abs(exp);

	while (b != 1)
	{
		if (b % 2 == 1) //uneven
		{
			C = C * A;
			--b;
		}
		b >>= 1; // /2
		A = A * A;
	}

	return A * C;
}

DXMatrix DXMatrix::Logarithm(void) const
{
	//compute X = log(this)
	static const double e1 = 1e-4f;
	static const int maxiter1 = 19;
	static const double e2 = 1e-9f;
	static const int maxiter2 = 7;

	int k = 0;
	DXMatrix A = *this;
	while ((A - IDENTITY()).Norm() > e1 && k < maxiter1)
	{
		float test = (A - IDENTITY()).Norm();

		A = A.Sqrt();
		k++;
	}

	A = A - IDENTITY();
	DXMatrix Z = A;
	DXMatrix X = A;

	int i = 1;
	while (Z.Norm() > e2 && i <= maxiter2)
	{
		float test = Z.Norm();

		Z *= A;
		X += Z / (float)++i;
	}
	X = ((float)(1<<k)) * X; // 2^k
	return X;
}

DXMatrix DXMatrix::Exponential(void) const
{
	static const int q = 6; //number of iterations
	
	//compute X = e^this

	int j = (int)_max(0, 1 + logn(2.f, Norm()));

	DXMatrix A = *this * 1.f / (1 << j); //scale by 1 / power of 2

	DXMatrix D = IDENTITY();
	DXMatrix N = IDENTITY();
	DXMatrix X = IDENTITY();

	float c = 1;
	for (int k = 1; k < q; k++)
	{
		c = c * (q - k + 1) / (k * ( 2 * q - k + 1));
		X = A * X;
		N = N + c * X;
		D = D + pow(-1.0f, k) * c * X;
	}
	
	X = N * InvertMatrix(D);
	X = X.Power(1 << j); //undo scaling
	return X;
}

DXMatrix DXMatrix::LerpDecomposed(const DXMatrix& A, const DXMatrix& B, const float s)
{
	DXVector3 scaleA, scaleB, scaleLerp;
	DXVector3 transA, transB, transLerp;
	D3DXQUATERNION rotA, rotB, rotLerp;

	D3DXMatrixDecompose((D3DXVECTOR3*)&scaleA, &rotA, (D3DXVECTOR3*)&transA, (D3DXMATRIX*)&A);
	D3DXMatrixDecompose((D3DXVECTOR3*)&scaleB, &rotB, (D3DXVECTOR3*)&transB, (D3DXMATRIX*)&B);
	
	scaleLerp = DXVector3::Lerp(scaleA, scaleB, s);
	transLerp = DXVector3::Lerp(transA, transB, s);
	D3DXQuaternionSlerp(&rotLerp, &rotA, &rotB, s);

	DXMatrix matLerp;
	D3DXMatrixTransformation((D3DXMATRIX*)&matLerp, NULL, NULL, (D3DXVECTOR3*)&scaleLerp, NULL, &rotLerp, (D3DXVECTOR3*)&transLerp);
	return matLerp;
}