/*
 * Copyright (C) 1999-2017 Hartmut Seichter
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#ifndef PW_CORE_MATRIX_HPP
#define PW_CORE_MATRIX_HPP

#include <pw/core/globals.hpp>
#include <pw/core/matrixbase.hpp>
#include <pw/core/math.hpp>

#include <numeric>


namespace pw {

template <unsigned int R, unsigned int C, typename T>
class matrix_ : public matrixbase<T> {

	T m[R*C];

public:

	using typename matrixbase<T>::value_type;
	using typename matrixbase<T>::size_type;

	matrix_();

	matrix_(const matrix_& mtc);

	matrix_& operator = (const matrix_& other);

	matrix_ transposed() const;

	inline matrix_& operator *= (const T& b) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) *= b; return *this; }
	inline matrix_& operator /= (const T& b) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) /= b; return *this; }
	inline matrix_& operator += (const T& b) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) += b; return *this; }
	inline matrix_& operator -= (const T& b) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) -= b; return *this; }

	inline matrix_& operator += (const matrix_& other) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) += other.at(i); return *this; }
	inline matrix_& operator -= (const matrix_& other) { for (unsigned int i = this->cells(); i--> 0;) this->at(i) -= other.at(i); return *this; }

	inline const matrix_ normalized() const {
		const T one_over_n = T(1) / this->norm();
		return *this * one_over_n;
	}


	inline const matrix_
	get_inverse() const
	{
		matrix_ resMat;

		for ( unsigned int r = 0; r < C; ++r) {
			for ( unsigned int j = 0; j < R; ++j) {
				short sgn = ( (r+j)%2) ? -1 : 1;
				matrix_<R-1,C-1,T> minor;
				this->get_minor(minor,r,j);
				resMat.at(r,j) = minor.determinant() * sgn;
			}
		}

		resMat = resMat.transposed();
		resMat *= (static_cast<T>(1)/this->determinant());
		return resMat;
	}

	inline
	matrix_& invert() {
		*this = this->get_inverse();
		return *this;
	}

	void get_minor(matrix_<R-1,C-1,T>& res, unsigned int r0, unsigned int c0) const;

	T determinant() const;

	T squared_norm() const;

	T norm() const;

	matrix_<R,C,T>& operator *= (const matrix_<R,C,T>& rhs);

	matrix_<R,C,T>& copy_from_data(const T* src) { for (unsigned int i = 0; i < R*C; ++i) { (*this).at(i) = src[i]; } return *this; }

	matrix_<R,C,T> operator * (const matrix_<R,C,T>& rhs) const {
		return mul(*this,rhs);
	}

	const matrix_<C,R,T> reshape() const {
		matrix_<C,R,T> m;
		for (unsigned int r = 0; r < R; ++r)
			for (unsigned int c = 0; c < C; ++c)
				m(r,c) = (*this)(c,r);
		return m;
	}

	const matrix_<R,1,T> get_column(unsigned int col) const {
		matrix_<R,1,T> c; for (unsigned int r = 0; r < R; ++r) c(r,0) = (this)(r,col);
		return c;
	}

	const matrix_<1,C,T> get_row(unsigned int row) const {
		matrix_<1,C,T> r; for (unsigned int c = 0; c < C; ++c) r(0,c) = (this)(row,c);
		return r;
	}

	void normalize();
};

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

template <unsigned int aR, unsigned int aC, typename T>
inline matrix_<aR,aC,T> operator * (const matrix_<aR,aC,T>& a, const T& b)
{
	matrix_<aR,aC,T> res;
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) * b;
	return res;
}

template <unsigned int aR, unsigned int aC, typename T>
inline matrix_<aR,aC,T> operator / (const matrix_<aR,aC,T>& a, const T& b)
{
	matrix_<aR,aC,T> res; T oneOverB(1./b);
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) * oneOverB;
	return res;
}

template <unsigned int aR, unsigned int aC, typename T>
inline matrix_<aR,aC,T> operator + (const matrix_<aR,aC,T>& a, const T& b)
{
	matrix_<aR,aC,T> res;
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) + b;
	return res;
}

template <unsigned int aR, unsigned int aC, typename T>
inline matrix_<aR,aC,T> operator - (const matrix_<aR,aC,T>& a, const T& b)
{
	matrix_<aR,aC,T> res;
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) - b;
	return res;
}

template <unsigned int R, unsigned int C, typename T>
inline matrix_<R,C,T> operator + (const matrix_<R,C,T>& a, const matrix_<R,C,T>& b)
{
	matrix_<R,C,T> res;
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) + b.at(i);
	return res;
}

template <unsigned int R, unsigned int C, typename T>
inline matrix_<R,C,T> operator - (const matrix_<R,C,T>& a, const matrix_<R,C,T>& b)
{
	matrix_<R,C,T> res;
	for (unsigned int i = res.cells(); i--> 0;) res.at(i) = a.at(i) - b.at(i);
	return res;
}

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

template <unsigned int aR,unsigned int aCbR, unsigned int bC, typename T>
matrix_<aR,bC,T> static inline
mul(const matrix_<aR,aCbR,T>& A, const matrix_<aCbR,bC,T>& B)
{
	// aC == bR
	// set all null
	matrix_<aR,bC,T> res;
	res.fill(0);

	// compute all resulting cells
	for (unsigned int r = 0; r < aR; ++r) {
		for (unsigned int c = 0; c < bC; ++c) {
			// building inner product
			for (unsigned int iI = 0; iI < aCbR;iI++) {
				res.at(r,c) += A.at(r,iI) * B.at(iI,c);
			}
		}
	}
	return res;
}



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

template <unsigned int R, unsigned int C,typename T>
matrix_<R,C,T>& matrix_<R,C,T>::operator *= (const matrix_& rhs)
{
	*this = mul(*this,rhs);
	return *this;
}

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

template <unsigned int R, unsigned int C,typename T>
matrix_<R,C,T>::matrix_()
	: matrixbase<T>(R,C,&m[0],true)
{
}

template <unsigned int R, unsigned int C,typename T>
matrix_<R,C,T>::matrix_(const matrix_ &mtc) :
	matrixbase<T>(R,C,&m[0],true)
{
	*this = mtc;
}

template <unsigned int R, unsigned int C,typename T>
matrix_<R,C,T> &matrix_<R,C,T>::operator = (const matrix_<R,C,T> &other)
{
	if (this != &other)
		for (unsigned int r = 0; r < R;++r)
			for (unsigned int c = 0; c < C;++c)
				(*this).at(r,c) = other.at(r,c);
	return *this;
}

template <unsigned int R, unsigned int C,typename T>
matrix_<R,C,T> matrix_<R,C,T>::transposed() const
{
	matrix_<C,R,T> res;
	for (unsigned int r = this->rows();r-->0;)
		for (unsigned int c = this->cols();c-->0;)
			res.at(c,r) = this->at(r,c);
	return res;
}

template <unsigned int R, unsigned int C,typename T>
void matrix_<R,C,T>::get_minor(matrix_<R-1,C-1,T>& res,unsigned int r0, unsigned int c0) const
{
	unsigned int r = 0;
	for (unsigned int ri = 0; ri < R; ri++)
	{
		unsigned int c = 0;
		if (ri == r0) continue;
		for (unsigned int ci = 0; ci < C; ci++)
		{
			if (ci == c0) continue;
			res.data()[r*(C-1)+c] = this->data()[ri*C + ci];//(*this)(ri,ci);
			c++;
		}
		r++;
	}
}

template <unsigned int R, unsigned int C,typename T> inline
T matrix_<R,C,T>::determinant() const
{
	T res(0);

	matrix_<R-1,C-1,T> minor;

	// using Laplace Expansion at compile time
	for (unsigned int c = 0; c < C; c++) {
		this->get_minor(minor,0,c);
		res += ((c % 2 == 0) ? m[c] : -m[c]) *  minor.determinant();
	}

	return res;
}

template <unsigned int R, unsigned int C,typename T> inline
T matrix_<R,C,T>::squared_norm() const
{
	T res(0);

	for (unsigned int r = 0; r < R; ++r)
		for (unsigned int c = 0; c < C; ++c)
			res += ((*this).at(r,c) * (*this).at(r,c));
	return res;
}


template <unsigned int R, unsigned int C,typename T> inline
T matrix_<R,C,T>::norm() const
{
	using std::sqrt;

	return sqrt(this->squared_norm());
}

template <unsigned int R, unsigned int C,typename T> inline
void matrix_<R,C,T>::normalize()
{
	T n = norm();
	if (n > 0) {
		for (unsigned int r = 0; r < R; ++r)
			for (unsigned int c = 0; c < C; ++c)
				(*this)(r,c) /= n;
	}
}

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

// explicit specializations

// 4x4

template <typename T>
class matrix44_ : public matrix_<4,4,T>
{
public:

	using matrix_<4,4,T>::matrix_;


	matrix44_(const matrix_<4,4,T>& i)
	{
		*this = i;
	}

	matrix44_& operator = (const matrix_<4,4,T>& rhs) {
		if (this != &rhs){
			this->at(0,0) = rhs.at(0,0);this->at(0,1) = rhs.at(0,1);this->at(0,2) = rhs.at(0,2);this->at(0,3) = rhs.at(0,3);
			this->at(1,0) = rhs.at(1,0);this->at(1,1) = rhs.at(1,1);this->at(1,2) = rhs.at(1,2);this->at(1,3) = rhs.at(1,3);
			this->at(2,0) = rhs.at(2,0);this->at(2,1) = rhs.at(2,1);this->at(2,2) = rhs.at(2,2);this->at(2,3) = rhs.at(2,3);
			this->at(3,0) = rhs.at(3,0);this->at(3,1) = rhs.at(3,1);this->at(3,2) = rhs.at(3,2);this->at(3,3) = rhs.at(3,3);
		}

		return *this;
	}

	inline static
	matrix_<4,4,T> projection_from_frustum(T Left,T Right,T Bottom,T Top,T zNear,T zFar)
	{
		matrix_<4,4,T> frustum;

		frustum.fill(0);

		frustum.at(0,0) = 2 * zNear/(Right-Left);
		frustum.at(1,1) = 2 * zNear/(Top-Bottom);

		frustum.at(0,2) =     (Right+Left)/(Right-Left);	//A
		frustum.at(1,2) =     (Top+Bottom)/(Top-Bottom);	//B
		frustum.at(2,2) = -   (zFar+zNear)/(zFar-zNear);	//C
		frustum.at(3,2) = -(2 * zFar*zNear)/(zFar-zNear);   //D

		frustum.at(2,3) = -1;

		return frustum;
	}

	inline static
	matrix_<4,4,T> orthogonal_projection(T Left, T Right,
										T Bottom,T Top,
										T Near, T Far)
	{

		matrix_<4,4,T> ortho;

		ortho.fill(0);
		ortho(0,0) =  2 / (Right-Left);
		ortho(1,1) =  2 / (Top-Bottom);
		ortho(2,2) = -2 / (Far-Near);

		ortho(0,3) = -(Right+Left)/(Right-Left);
		ortho(1,3) = -(Top+Bottom)/(Top-Bottom);
		ortho(2,3) = -(Far+Near)/(Far-Near);

		ortho(3,3) = 1;

		return ortho;
	}


	inline static
	matrix_<4,4,T> perspective_projection(T fovY, T aspectRatio, T zNear, T zFar)
	{
		const T height = zNear * tan(fovY/T(360) * pi<T>());  // half height of near plane
		const T width = height * aspectRatio;             // half width of near plane

		return projection_from_frustum(-width, width, -height, height, zNear, zFar );
	}


#if TACIT_PIXEL_STUFF_NEEDS_TO_MOVE_TO_SCENE

	matrix<4,4,T>&
	translate(const T& v1,const T& v2,const T& v3)
	{
		this->at(12) += v1;
		this->at(13) += v2;
		this->at(14) += v3;
		return *this;
	}

	matrix<4,4,T>&
	setTranslation(const T& v1,const T& v2,const T& v3)
	{
		this->identity();
		this->at(12) = v1;
		this->at(13) = v2;
		this->at(14) = v3;
		return *this;
	}

	matrix<4,4,T>&
	setScale(const T& v1,const T& v2,const T& v3)
	{
		this->identity();
		(*this)(0,0) = v1;
		(*this)(1,1) = v2;
		(*this)(2,2) = v3;
		return *this;
	}

	matrix<4,4,T>&
	scale(const T& v1,const T& v2,const T& v3)
	{
		(*this)(0,0) *= v1;
		(*this)(1,1) *= v2;
		(*this)(2,2) *= v3;
		return *this;
	}


	static
	matrix<4,4,T>
	Translation(const T& v1,const T& v2,const T& v3)
	{
		matrix44<T> res = matrix44<T>::Identity(); res.setTranslation(v1,v2,v3);
		return res;
	}

	inline static
	matrix<4,4,T>
	AngleAxis(const T& radianRotation,const matrix<3,1,T>& vec);



	inline static
	matrix<4,4,T> Frustum(T Left,T Right,T Bottom,T Top,T zNear,T zFar)
	{
		matrix<4,4,T> frustum;

		frustum.fill(0);

		frustum(0,0) = 2 * zNear/(Right-Left);
		frustum(1,1) = 2 * zNear/(Top-Bottom);

		frustum(0,2) =     (Right+Left)/(Right-Left);	//A
		frustum(1,2) =     (Top+Bottom)/(Top-Bottom);	//B
		frustum(2,2) = -   (zFar+zNear)/(zFar-zNear);	//C
		frustum(3,2) = -(2 * zFar*zNear)/(zFar-zNear);   //D

		frustum(2,3) = -1;

		return frustum;
	}


	inline static
	matrix<4,4,T> LookAt(const matrix<3,1,T>& eye,
						 const matrix<3,1,T>& target,
						 const matrix<3,1,T>& up);


	matrix<4,4,T>&
	rotate(const matrix<3,1,T>& vec, const T& rotation)
	{
		matrix44<T> rot = matrix44<T>::AngleAxis(rotation,vec); *this *= rot;
		return *this;
	}

#endif


};

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

//
// Specializations
//

template <> inline
float matrix_<1,1,float>::determinant() const
{
	return this->at(0);
}

template <> inline
double matrix_<1,1,double>::determinant() const
{
	return this->at(0);
}


#if TACIT_PIXEL_STUFF_NEEDS_TO_MOVE_TO_SCENE

template <typename T>
class matrix31 : public matrix<3,1,T> {
public:

	using matrix<3,1,T>::operator =;

	inline static
	matrix<3,1,T> Cross(const matrix<3,1,T>& vec1, const matrix<3,1,T>& vec2)
	{
		matrix<3,1,T> res;

		res.at(0) = vec1.at(1) * vec2.at(2) - vec2.at(1) * vec1.at(2);
		res.at(1) = vec1.at(2) * vec2.at(0) - vec2.at(2) * vec1.at(0);
		res.at(2) = vec1.at(0) * vec2.at(1) - vec2.at(0) * vec1.at(1);

		return res;
	}
};

template <typename T>
matrix<4,4,T>
matrix44<T>::AngleAxis(const T &radianRotation, const matrix<3,1,T> &vec)
{
	matrix44<T> R = matrix44<T>::Identity();

	if (vec.norm() < std::numeric_limits<T>::epsilon()) return R;

	T _fCos = (T) cos (radianRotation);

	matrix<3,1,T> _vCos(vec * (1 - _fCos));
	matrix<3,1,T> _vSin(vec * (T)sin(radianRotation));

	R.at(0) = (T) ((vec(0,0) * _vCos(0,0)) + _fCos);
	R.at(4) = (T) ((vec(0,0) * _vCos(1,0)) - _vSin(2,0));
	R.at(8) = (T) ((vec(0,0) * _vCos(2,0)) + _vSin(1,0));

	R.at(1) = (T) ((vec(1,0) * _vCos(0,0)) + _vSin(2,0));
	R.at(5) = (T) ((vec(1,0) * _vCos(1,0)) + _fCos);
	R.at(9) = (T) ((vec(1,0) * _vCos(2,0)) - _vSin(0,0));

	R.at(2) = (T)  ((vec(2,0) * _vCos(0,0)) - _vSin(1,0));
	R.at(6) = (T)  ((vec(2,0) * _vCos(1,0)) + _vSin(0,0));
	R.at(10)= (T)  ((vec(2,0) * _vCos(2,0)) + _fCos);

	return R;
}

template <typename T>
matrix<4,4,T>
matrix44<T>::LookAt(const matrix<3,1,T> &eye, const matrix<3,1,T> &target, const matrix<3,1,T> &up)
{
	matrix<4,4,T> lookat = matrix<4,4,T>::Identity();

	matrix<3,1,T> L; L = target - eye;
	L.normalize();
	matrix<3,1,T> S = matrix31<T>::Cross(L,up);
	S.normalize();
	matrix<3,1,T> Ud = matrix31<T>::Cross(S,L);
	Ud.normalize();

	lookat(0,0) = S.at(0);
	lookat(0,1) = S.at(1);
	lookat(0,2) = S.at(2);
	lookat(0,3) = T(0);

	lookat(1,0) = Ud.at(0);
	lookat(1,1) = Ud.at(1);
	lookat(1,2) = Ud.at(2);
	lookat(1,3) = T(0);

	lookat(2,0) = -L.at(0);
	lookat(2,1) = -L.at(1);
	lookat(2,2) = -L.at(2);
	lookat(3,2) = T(0);

	lookat(3,0) = eye.at(0);
	lookat(3,1) = eye.at(1);
	lookat(3,2) = eye.at(2);
	lookat(3,3) = 1;

	return lookat;

}

#endif


// predefined matricies

typedef matrix44_<real_t> matrix44;

typedef matrix44_<double> matrix44d;
typedef matrix44_<float> matrix44f;

}



#endif


