#pragma once

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

template<class T, const unsigned ROW, const unsigned COL>
class Matrix
{
	T m[COL][ROW];

public:
	Matrix(void) { ZeroMemory(m, sizeof(T) * ROW * COL); }
	~Matrix(void) {}

	static const unsigned GetRowCount() { return ROW; }
	static const unsigned GetColCount() { return COL; }
	
	// access grants
	T operator()(const unsigned Row, const unsigned Col) const { return m[Row][Col]; }
	T& operator()(const unsigned Row, const unsigned Col) { return m[Row][Col]; }

	// assignment operators
    Matrix<T, ROW, COL>& operator *= (const Matrix<T, ROW, COL>& A)
	{
		//have to store temp, to avoid overwriting when A *= A
		Matrix<T, ROW, COL> temp;
		
		for(unsigned r = 0; r < ROW; r++) 
		{
			for(unsigned c = 0; c < COL; c++) 
			{
				T& v = temp(r, c);
				v = 0;

				//row * col
				for(unsigned i = 0; i < _max(COL, ROW); i++) 
				{
					v += m[r][i] * A(i, c);
				}
			}
		}
		return *this = temp;
	}

    Matrix<T, ROW, COL>& operator += (const Matrix<T, ROW, COL>& A)
	{
		for (int r = 0; r < ROW; r++)
		{
			for (int c = 0; c < COL; c++)
			{
				m[r][c] += A(r, c);
			}
		}
		return *this;
	}

    Matrix<T, ROW, COL>& operator -= (const Matrix<T, ROW, COL>& A)
	{
		for (int r = 0; r < ROW; r++)
		{
			for (int c = 0; c < COL; c++)
			{
				m[r][c] -= A(r, c);
			}
		}
		return *this;
	}

    Matrix<T, ROW, COL>& operator *= (const float s)
	{
		for (int r = 0; r < ROW; r++)
		{
			for (int c = 0; c < COL; c++)
			{
				m[r][c] *= s;
			}
		}
		return *this;
	}

    Matrix<T, ROW, COL>& operator /= (const float r)
	{
		return operator*=(1 / r);
	}

    // unary operators
    Matrix<T, ROW, COL> operator + () const
	{
		return *this;	
	}

    Matrix<T, ROW, COL> operator - () const
	{
		Matrix<T, ROW, COL> mat;
		for (int r = 0; r < ROW; r++)
		{
			for (int c = 0; c < COL; c++)
			{
				mat(r, c) *= -m[r][c];
			}
		}
		return mat;
	}

	// binary operators
    Matrix<T, ROW, COL> operator * (const Matrix<T, ROW, COL>& A) const
	{
		Matrix<T, ROW, COL> temp;
		
		for(unsigned r = 0; r < ROW; r++) 
		{
			for(unsigned c = 0; c < COL; c++) 
			{
				T& v = temp(r,c);
				v = 0;

				//row * col
				for(unsigned i = 0; i < _max(COL, ROW); i++) 
				{
					v += m[r][i] * A(i, c);
				}
			}
		}
		return temp;
	}

    Matrix<T, ROW, COL> operator + (const Matrix<T, ROW, COL>& A) const
	{
		return Matrix<T, ROW, COL>(*this) += A;
	}

    Matrix<T, ROW, COL> operator - (const Matrix<T, ROW, COL>& A) const
	{
		return Matrix<T, ROW, COL>(*this) -= A;
	}

    Matrix<T, ROW, COL> operator * (const float s) const
	{
		return Matrix<T, ROW, COL>(*this) *= s;
	}

    Matrix<T, ROW, COL> operator / (const float r) const
	{
		return Matrix<T, ROW, COL>(*this) *= 1 / r;
	}

	friend Matrix<T, ROW, COL> operator * (const T s, const Matrix<T, ROW, COL>& A)
	{
		return A * s;
	}

	//functions
	T Norm(void) const
	{
		T maxabs = 0;
		for (int r = 0; r < ROW; r++)
		{
			for (int c = 0; c < COL; c++)
			{
				maxabs = _max(maxabs, m[r][c]);
			}
		}
		return ROW * COL * maxabs;
	}

	T Det(void) const
	{
		//matrix must be NxN
		//using Sarrus' scheme
		T s = 0;
		for (int c = 0; c < COL; c++)
		{
			T ppos = 1;
			T pneg = -1;
			for (int roff = 0; roff < ROW; roff++)
			{
				ppos *= m[roff][(c + roff) % ROW];
				pneg *= m[roff][(ROW - 1 + c - roff) % ROW];
			}
			s += ppos + pneg;
		}
		
		return s;
	}

	//matrix constructions
	static Matrix<T, ROW, COL> IDENTITY(void)
	{
		Matrix<T, ROW, COL> mx; //all zero

		for (int i = 0; i < ROW && i < COL; i++)
		{
			mx(i, i) = 1;
		}

		return mx;
	}

	static Matrix<T, ROW, COL> InvertMatrix(const Matrix<T, ROW, COL>& mat)
	{
		Matrix<T, ROW, COL> mx;

		//using Gauss-Jordan Elimination
		//hm... TODO

		if (ROW == 4 && COL == 4) //copy to dxmatrix, do inversion, copy back and return
		{	
			DXMatrix matEasy;
			for (int r = 0; r < 4; r++)
			{
				for (int c = 0; c < 4; c++)
				{
					matEasy(r, c) = mat(r, c);
				}
			}

			matEasy.Invert();

			for (int r = 0; r < 4; r++)
			{
				for (int c = 0; c < 4; c++)
				{
					mx(r, c) = matEasy(r, c);
				}
			}
		}

		return mx;
	}

	//operations
	Matrix<T, ROW, COL> Sqrt(void) const
	{
		//compute X = A^(1/2)
		static const double e = 1e-6f;
		static const int maxiter = 10;

		Matrix<T, ROW, COL> X = *this;
		Matrix<T, ROW, COL> Y = IDENTITY();
	
		int i = 0; //limit maximum iterations to avoid infinite looping
		while ((X*X - *this).Norm() > e && i++ < maxiter)
		{
			T test = (X*X - *this).Norm();
			Matrix<T, ROW, COL> iX = InvertMatrix(X);
			Matrix<T, ROW, COL> iY = InvertMatrix(Y);
			X = 0.5 * (X + iY);
			Y = 0.5 * (Y + iX);
		}
		return X;
	}

	Matrix<T, ROW, COL> Power(const int exp) const
	{
		Matrix<T, ROW, COL> C = IDENTITY();
		if (exp == 0)
			return C;

		Matrix<T, ROW, COL> 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;
	}

	//log and exp from paper: Camera Keyframing Using Linear Interpolation of Matrices
	Matrix<T, ROW, COL> Logarithm(void) const
	{
		//compute X = log(this)
		static const T e1 = 1e-4f;
		static const int maxiter1 = 20;
		static const T e2 = 1e-9f;
		static const int maxiter2 = 7;

		int k = 0;
		Matrix<T, ROW, COL> A = *this;
		while ((A - IDENTITY()).Norm() > e1 && k < maxiter1)
		{
			T test = (A - IDENTITY()).Norm();

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

		A = A - IDENTITY();
		Matrix<T, ROW, COL> Z = A;
		Matrix<T, ROW, COL> X = A;

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

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

	Matrix<T, ROW, COL> 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()));

		Matrix<T, ROW, COL> A = *this * 1.f / (1 << j); //scale by 1 / power of 2

		Matrix<T, ROW, COL> D = IDENTITY();
		Matrix<T, ROW, COL> N = IDENTITY();
		Matrix<T, ROW, COL> X = IDENTITY();

		T 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;
	}

	static Matrix<T, ROW, COL> AlexaScale(const Matrix<T, ROW, COL>& A, const T s)
	{
		Matrix<T, ROW, COL> mlog = A.Logarithm();
		return (mlog * s).Exponential();
	}

	static Matrix<T, ROW, COL> AlexaAdd(const Matrix<T, ROW, COL>& A, const Matrix<T, ROW, COL>& B)
	{
		return (A.Logarithm() + B.Logarithm()).Exponential();
	}

	static Matrix<T, ROW, COL> Lerp(const Matrix<T, ROW, COL>& A, const Matrix<T, ROW, COL>& B, const T s)
	{
		Matrix<T, ROW, COL> a = AlexaScale(A, 1 - s);
		Matrix<T, ROW, COL> b = AlexaScale(B, s);
		return AlexaAdd(a, b);
	}
};

typedef Matrix<double, 3, 3> Matrix3x3;
typedef Matrix<float, 3, 3> Matrix3x3f;
typedef Matrix<double, 4, 4> Matrix4x4;
typedef Matrix<float, 4, 4> Matrix4x4f;
