#pragma once

#include "vector3.h"

#include <limits>



namespace lm
{


//! 3次正方行列
/*

 インデックスは数学の行列と一致
 [ (0,0) , (0,1) , (0,2) ]
 [ (1,0) , (1,1) , (1,2) ]
 [ (2,0) , (2,1) , (2,2) ]

 通しインデックスは
 [ 0 , 1 , 2 ]
 [ 3 , 4 , 5 ]
 [ 6 , 7 , 8 ]

 積の結合方向は

 [ax] = [bx by bz] * [ m00 m01 m02 ]
 [ay]                [ m10 m11 m12 ]
 [az]                [ m20 m21 m22 ]

 [ax] = [ m00 m01 m02 ] * [bx]
 [ay]   [ m10 m11 m12 ]   [by]
 [az]   [ m20 m21 m22 ]   [bz]

 ベクトルの行列変換の正方向は
 v' = v * m;

*/
template<typename T>
class matrix3
{
public:
	enum
	{
		WIDTH        = 3 , //!< 行,列の幅
		NUM_ELEMENTS = 9 , //!< 全要素数
	};

public:
	T m00 , m01 , m02 ,
	  m10 , m11 , m12 ,
	  m20 , m21 , m22 ;

public:
	matrix3(void)
		: m00(0) , m01(0) , m02(0)
		, m10(0) , m11(0) , m12(0)
		, m20(0) , m21(0) , m22(0)
	{}

	matrix3(const T* _v)
		: m00(_v[0]) , m01(_v[1]) , m02(_v[2])
		, m10(_v[3]) , m11(_v[4]) , m12(_v[5])
		, m20(_v[6]) , m21(_v[7]) , m22(_v[8])
	{}

	matrix3(const matrix3<T>& m)
		: m00(m.m00) , m01(m.m01) , m02(m.m02)
		, m10(m.m10) , m11(m.m11) , m12(m.m12)
		, m20(m.m20) , m21(m.m21) , m12(m.m22)
	{}


	// バッファの先頭を直接参照
	T*       v(void)       { return &m00; }
	const T* v(void) const { return &m00; }


	// 通しインデックスで要素取得
	      T& operator[](size_t idx);
	const T& operator[](size_t idx) const;
	      T& operator()(size_t idx);
	const T& operator()(size_t idx) const;

	      T& at(size_t idx);
	const T& at(size_t idx) const;


	// row(行) , col(列) のインデックスで要素取得
	      T& operator()(size_t idx_row, size_t idx_col);
	const T& operator()(size_t idx_row, size_t idx_col) const;

	      T& at(size_t idx_row, size_t idx_col);
	const T& at(size_t idx_row, size_t idx_col) const;


	void set( const matrix3<T>& m );
	void set( const T* _v );
	void set( const T& _v00 , const T& _v01 , const T& _v02 ,
	          const T& _v10 , const T& _v11 , const T& _v12 ,
	          const T& _v20 , const T& _v21 , const T& _v22 );

	matrix3<T>& operator=( const matrix3<T>& m );

	// 全要素に同じ値をセットする
	void fill( const T& _v );

	//! 単位行列化
	void set_identity(void);
	//! 零行列化
	void set_zero(void);

	//! 単位行列生成
	static const matrix3<T>& get_identity(void);
	//! 零行列生成
	static const matrix3<T>& get_zero(void);

	//! 転置
	void transpose(void);
	matrix3<T> get_transpose(void) const;

	//! 逆行列
	void invert(void);
	matrix3<T> get_invert(void) const;

	T get_det(void) const;

	bool equals( const matrix3<T>& m ) const;
	bool equals( const T* _v ) const;
	bool operator==( const matrix3<T>& m ) const;
	bool operator!=( const matrix3<T>& m ) const;


	matrix3<T>& operator+=( const matrix3<T>& m );
	matrix3<T>& operator-=( const matrix3<T>& m );
	matrix3<T>& operator*=( const float& f );
	matrix3<T>& operator/=( const float& f );

	matrix3<T>& operator*=( const matrix3<T>& m );
};

typedef matrix3<float>  matrix3f;
typedef matrix3<double> matrix3d;



// global method

template<typename T>
matrix3<T> operator+( const matrix3<T>& m1 , const matrix3<T>& m2 );
template<typename T>
matrix3<T> operator-( const matrix3<T>& m1 , const matrix3<T>& m2 );
template<typename T>
matrix3<T> operator*( const matrix3<T>& m1 , const matrix3<T>& m2 );

template<typename T>
matrix3<T> operator*( const matrix3<T>& m1 , const T& val );
template<typename T>
matrix3<T> operator*( const T& val , const matrix3<T>& m1 );

template<typename T>
matrix3<T> operator/( const matrix3<T>& m1 , const T& val );

// vector3 との積
// ※ { v *= m } == { v = v * m } != { v = m * v }
template<typename T>
vector3<T>& operator*=( vector3<T>& v , const matrix3<T>& m );
template<typename T>
vector3<T> operator*( const vector3<T>& v , const matrix3<T>& m );

template<typename T>
vector3<T> operator*( const matrix3<T>& m , const vector3<T>& v );



// matrix3 implementation

template<typename T> inline
T& matrix3<T>::operator[](size_t idx)
{
	return at(idx);
}
template<typename T> inline
const T& matrix3<T>::operator[](size_t idx) const
{
	return at(idx);
}
template<typename T> inline
T& matrix3<T>::operator()(size_t idx)
{
	return at(idx);
}
template<typename T> inline
const T& matrix3<T>::operator()(size_t idx) const 
{
	return at(idx);
}
template<typename T> inline
T& matrix3<T>::operator()(size_t idx_row, size_t idx_col)
{
	return at( idx_row , idx_col );
}
template<typename T> inline
const T& matrix3<T>::operator()(size_t idx_row, size_t idx_col) const
{
	return at( idx_row , idx_col );
}

template<typename T> inline
T& matrix3<T>::at(size_t idx)
{
	return v()[idx];
}

template<typename T> inline
const T& matrix3<T>::at(size_t idx) const
{
	return v()[idx];
}

template<typename T> inline
T& matrix3<T>::at(size_t idx_row, size_t idx_col)
{
	return v()[ idx_col + idx_row * WIDTH ];
}

template<typename T> inline
const T& matrix3<T>::at(size_t idx_row, size_t idx_col) const
{
	return v()[ idx_col + idx_row * WIDTH ];
}


template<typename T> inline
void matrix3<T>::set( const matrix3<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = m.at(i);
}

template<typename T> inline
void matrix3<T>::set( const T* _v )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = _v[i];
}

template<typename T> inline
void matrix3<T>::set( const T& _v00 , const T& _v01 , const T& _v02 ,
                      const T& _v10 , const T& _v11 , const T& _v12 ,
                      const T& _v20 , const T& _v21 , const T& _v22 )
{
	m00 = _v00;   m01 = _v01;   m02 = _v02;
	m10 = _v10;   m11 = _v11;   m12 = _v12;
	m20 = _v20;   m21 = _v21;   m22 = _v22;
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator=( const matrix3<T>& m )
{
	set(m);
	return *this;
}


template<typename T> inline
void matrix3<T>::fill( const T& _v )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = _v;
}


template<typename T> inline
void matrix3<T>::set_identity(void)
{
	set( get_identity() );
}

template<typename T> inline
void matrix3<T>::set_zero(void)
{
	set( get_zero() );
}

template<typename T> inline
const matrix3<T>& matrix3<T>::get_identity(void)
{
	static const T v[NUM_ELEMENTS]
		= { T(1) , T(0) , T(0) , 
		    T(0) , T(1) , T(0) , 
		    T(0) , T(0) , T(1) };

	static const matrix3<T> m(v);
	return m;
}

template<typename T> inline
const matrix3<T>& matrix3<T>::get_zero(void)
{
	static const T v[NUM_ELEMENTS]
		= { T(0) , T(0) , T(0) , 
		    T(0) , T(0) , T(0) , 
		    T(0) , T(0) , T(0) };

	static const matrix3<T> m(v);
	return m;
}

template<typename T> inline
void matrix3<T>::transpose(void)
{
	(std::swap)( m01 , m10 );
	(std::swap)( m02 , m20 );
	(std::swap)( m12 , m21 );
}

template<typename T> inline
matrix3<T> matrix3<T>::get_transpose(void) const
{
	matrix3<T> m = (*this);
	m.transpose();
	return m;
}

template<typename T> inline
T matrix3<T>::get_det(void) const
{
	T det =
		+ at(0,0)*(at(1,1)*at(2,2) - at(2,1)*at(1,2))
		+ at(1,0)*(at(2,1)*at(0,2) - at(0,1)*at(2,2))
		+ at(2,0)*(at(0,1)*at(1,2) - at(1,1)*at(0,2));

	return det;
}

template<typename T> inline
void matrix3<T>::invert(void)
{
	matrix3<T> inv_mat;

	T det = get_det();
	if (det <= (std::numeric_limits<T>::min)())
	{
		(*this) = inv_mat;
		return;
	}

	inv_mat(0,0) = at(1,1)*at(2,2) - at(1,2)*at(2,1);
	inv_mat(0,1) = at(0,2)*at(2,1) - at(0,1)*at(2,2);
	inv_mat(0,2) = at(0,1)*at(1,2) - at(0,2)*at(1,1);
	inv_mat(1,0) = at(1,2)*at(2,0) - at(1,0)*at(2,2);
	inv_mat(1,1) = at(0,0)*at(2,2) - at(0,2)*at(2,0);
	inv_mat(1,2) = at(0,2)*at(1,0) - at(0,0)*at(1,2);
	inv_mat(2,0) = at(1,0)*at(2,1) - at(1,1)*at(2,0);
	inv_mat(2,1) = at(0,1)*at(2,0) - at(0,0)*at(2,1);
	inv_mat(2,2) = at(0,0)*at(1,1) - at(0,1)*at(1,0);

	T det_inv = T(1) / det;

	inv_mat *= det_inv;

	(*this) = inv_mat;
}

template<typename T> inline
matrix3<T> matrix3<T>::get_invert(void) const
{
	matrix3<T> m = (*this);
	m.invert();
	return m;
}


template<typename T> inline
bool matrix3<T>::equals( const matrix3<T>& m ) const
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		if( m.at(i) != at(i) ) return false;
	return true;
}

template<typename T> inline
bool matrix3<T>::equals( const T* _v ) const
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		if( m[i] != at(i) ) return false;
	return true;
}

template<typename T> inline
bool matrix3<T>::operator==( const matrix3<T>& m ) const
{
	return equals( m );
}

template<typename T> inline
bool matrix3<T>::operator!=( const matrix3<T>& m ) const
{
	return !equals( m );
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator+=( const matrix3<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) += m[i];

	return *this;
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator-=( const matrix3<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) -= m[i];

	return *this;
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator*=( const float& val )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) *= val;

	return *this;
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator/=( const float& val )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) /= val;

	return *this;
}

template<typename T> inline
matrix3<T>& matrix3<T>::operator*=( const matrix3<T>& m )
{
	matrix3<T> tmp;

	tmp.m00 = this->m00 * m.m00 + this->m01 * m.m10 + this->m02 * m.m20;
	tmp.m10 = this->m10 * m.m00 + this->m11 * m.m10 + this->m12 * m.m20;
	tmp.m20 = this->m20 * m.m00 + this->m21 * m.m10 + this->m22 * m.m20;

	tmp.m01 = this->m00 * m.m01 + this->m01 * m.m11 + this->m02 * m.m21;
	tmp.m11 = this->m10 * m.m01 + this->m11 * m.m11 + this->m12 * m.m21;
	tmp.m21 = this->m20 * m.m01 + this->m21 * m.m11 + this->m22 * m.m21;

	tmp.m02 = this->m00 * m.m02 + this->m01 * m.m12 + this->m02 * m.m22;
	tmp.m12 = this->m10 * m.m02 + this->m11 * m.m12 + this->m12 * m.m22;
	tmp.m22 = this->m20 * m.m02 + this->m21 * m.m12 + this->m22 * m.m22;

	(*this) = tmp;

	return *this;
}



// global method implements

template<typename T> inline
matrix3<T> operator+( const matrix3<T>& m1 , const matrix3<T>& m2 )
{
	matrix3<T> ret = m1;
	ret += m1;
	return ret;
}

template<typename T> inline
matrix3<T> operator-( const matrix3<T>& m1 , const matrix3<T>& m2 )
{
	matrix3<T> ret = m1;
	ret -= m1;
	return ret;
}

template<typename T> inline
matrix3<T> operator*( const matrix3<T>& m1 , const matrix3<T>& m2 )
{
	matrix3<T> ret = m1;
	ret *= m2;
	return ret;
}

template<typename T> inline
matrix3<T> operator*( const matrix3<T>& m1 , const T& val )
{
	matrix3<T> ret = m1;
	ret *= val;
	return ret;
}

template<typename T> inline
matrix3<T> operator*( const T& val , const matrix3<T>& m1 )
{
	matrix3<T> ret = m1;
	ret *= val;
	return ret;
}

template<typename T> inline
matrix3<T> operator/( const matrix3<T>& m1 , const T& val )
{
	matrix3<T> ret = m1;
	ret /= val;
	return ret;
}

template<typename T> inline
vector3<T>& operator*=( vector3<T>& v , const matrix3<T>& m )
{
	vector3<T> tmp = v;

	v.x = tmp.x * m.m00 + tmp.y * m.m10 + tmp.z * m.m20;
	v.y = tmp.x * m.m01 + tmp.y * m.m11 + tmp.z * m.m21;
	v.z = tmp.x * m.m02 + tmp.y * m.m12 + tmp.z * m.m22;

	return v;
}

template<typename T> inline
vector3<T> operator*( const vector3<T>& v , const matrix3<T>& m )
{
	vector3<T> ret = v;
	ret *= m;
	return ret;
}

template<typename T> inline
vector3<T> operator*( const matrix3<T>& m , const vector3<T>& v )
{
	vector3<T> ret;
	ret.x = v.x * m.m00 + v.y * m.m01 + v.z * m.m02;
	ret.y = v.x * m.m10 + v.y * m.m11 + v.z * m.m12;
	ret.z = v.x * m.m20 + v.y * m.m21 + v.z * m.m22;
	return ret;
}


}
