#pragma once
#include <mof/base/mofdef.hpp>
#include <mof/math/threshold.hpp>
#include <boost/operators.hpp>
#include <boost/utility.hpp>
#include <ostream>
#include <iomanip>
#include <cmath>

namespace mof
{
namespace math
{
	template <size_t Dim> class row_of_matrix;

	/**
	 * @brief 同次座標変換行列テンプレートクラス
	 * @note  このクラスはアフィン変換行列を表す
	 * @note  このテンプレートから直接特殊化することは想定していない．
	 * あくまでmatrixxを実装するための補助テンプレートである．
	 * このクラスは不変クラスである．
	 * @tparam Dim        行列の次元(要素数はこの数値の2乗)
	 * @tparam Derived    特殊化されたテンプレートの派生クラス(matrixx)の型
	 * @tparam Coordinate 対応するベクトルクラス(vectorx)の型
	 */
	template <size_t Dim, typename Derived, typename Coordinate>
	class basic_matrix
		: boost::addable< Derived 
		, boost::addable2< Derived, float
		, boost::subtractable< Derived 
		, boost::subtractable2< Derived, float
		, boost::multipliable2< Derived, float
		, boost::dividable2< Derived, float
		, boost::equality_comparable< Derived
		> > > > > > >
	{
	protected:
//{{{ size
		/**
		 * @brief elements_のサイズを返す
		 */
		size_t size() const
		{
			return Dim * (Dim + 1);
		}
//}}}
		// <実装メモ>elements_は1次元配列として提供している．
		// 無名共用体を使って2次元配列を用意することもできるが、FPUレジスタの内容を
		// 毎回スタックと同期しようとするらしく、速度が大幅に落ちてしまう．
		float elements_[Dim * (Dim + 1)];///< 要素の配列(アフィン行列なので，最後の行は省略している)
	public:
		// コンストラクタ，デストラクタはデフォルトのものを使う
		// 代入演算子，コピーコンストラクタはデフォルトのものを使う
//{{{ swap
		void swap(Derived& rhs) throw()
		{
			using std::swap;
			for (size_t i = 0; i < size(); ++i) {
				swap(elements_[i], rhs.elements_[i]);
			}
		}
//}}}
//{{{ operator +=
		Derived& operator+=(const Derived& rhs)
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] += rhs.elements_[i];
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
		
		Derived& operator+=(float rhs) 
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] += rhs;
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
		
		friend Derived operator+(float rhs1, Derived& rhs2)
		{
			float tmp[size()];
			for (size_t i = 0; i < size(); ++i) {
				tmp[i] = rhs1 + rhs2.elements_[i];
			}
			return Derived(tmp);
		}
//}}}
//{{{ operator -=
		Derived& operator-=(const Derived& rhs)
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] -= rhs.elements_[i];
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
		
		Derived& operator-=(float rhs)
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] -= rhs;
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
		
		friend Derived operator-(float rhs1, Derived& rhs2) 
		{
			float tmp[size()];
			for (size_t i = 0; i < size(); ++i) {
				tmp[i] = rhs1 - rhs2.elements_[i];
			}
			return Derived(tmp);
		}
//}}}
//{{{ operator *=	
		Derived operator*(const Derived& rhs)
		{
			Derived M;
			for (size_t i = 0; i < Dim; ++i) {
				for (size_t j = 0; j <= Dim; ++j) {
					float sum = 0;
					for (size_t k = 0; k <= Dim; ++k) {
						sum += at(i, k) * rhs.at(k, j);
					}
					M.elements_[i * (Dim+1) + j] = sum;
				}
			}
			return M;
		}
		
		Derived& operator*=(const Derived& rhs)
		{
			Derived M;
			for (size_t i = 0; i < Dim; ++i) {
				for (size_t j = 0; j <= Dim; ++j) {
					float sum = 0;
					for (size_t k = 0; k <= Dim; ++k) {
						sum += at(i, k) * rhs.at(k, j);
					}
					M.elements_[i * (Dim+1) + j] = sum;
				}
			}
			*this = M;
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}


		Derived& operator*=(float rhs) 
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] *= rhs;
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
	
		Coordinate operator*(const Coordinate& rhs)
		{
			float arr[Dim];
			for (size_t i = 0; i < Dim; ++i) {
				float sum = 0;
				for (size_t k = 0; k <= Dim; ++k) {
					sum += elements_[i * (Dim+1) + k] * rhs[k];
				}
				arr[i] = sum;
			}
			return Coordinate(arr);
		}
//}}}
//{{{ operator /=
		Derived& operator/=(float rhs)
		{
			for (size_t i = 0; i < size(); ++i) {
				elements_[i] /= rhs;
			}
			return *reinterpret_cast<Derived*>(this);//thisがDerived型であることは保証されている．
		}
//}}}
//{{{ operator ==
		bool operator==(const Derived& rhs) const
		{
			for (size_t i = 0; i < size(); ++i) {
				if (std::abs(elements_[i] - rhs.elements_[i]) > MOF_ERROR_THRESHOLD) return false;
			}
			return true;
		}
//}}}
//{{{ operator []
		/**
		 * @note M[i][j]のように参照可能
		 * @note この方法による複数の要素への参照は非効率
		 */
		row_of_matrix<Dim> const operator [](size_t i) const
		{
			return row_of_matrix<Dim>(i, elements_);
		}
//}}}
//{{{ operator <<
		/**
		 * @brief デバッグ出力用ストリーム演算子
		 * @param [in] stream 出力ストリーム
		 * @param [in] rhs    出力対象となる行列オブジェクト
		 * @return 引数で与えられた出力ストリーム
		 */
		friend std::ostream& operator<<
		(
			std::ostream& stream,
			const Derived& rhs
		)
		{
			for (size_t i = 0; i < Dim + 1; ++i) {
				for (size_t j = 0; j < Dim + 1; ++j) {
					if (j != 0) stream << ", ";
					else if (j != Dim) stream << "\n";
					stream << std::setw(5) << rhs[i][j];
				}
			}
			return stream;
		}
//}}}
//{{{ at
		/**
		 * @brief 行列の要素にアクセス
		 * @param[in] 行番号
		 * @param[in] 列番号
		 * @return    要素
		 */
		const float at(size_t i, size_t j) const
		{
			if (i < Dim ) return elements_[i * (Dim+1) + j];
			if (j == Dim) return 1; 
			else return 0;
		}
//}}}
	};
//{{{ swap
	template <size_t Dim, typename Derived, typename Coordinate>
	void swap
	(
		basic_matrix<Dim, Derived, Coordinate>& a,
		basic_matrix<Dim, Derived, Coordinate>& b
	) throw()
	{
		a.swap(b);
	}
//}}}
//{{{ row_of_matrix
	/**
	 * @brief M[i][j]のように行列の要素を取得するための補助クラス
	 */
	template <size_t Dim>
	class row_of_matrix : boost::noncopyable
	{
		size_t index_;
		const float (&elements_)[Dim * (Dim+1)];

	public:
		row_of_matrix(size_t i, const float (&elements)[Dim * (Dim+1)])
			: index_(i), elements_(elements)
		{
		}

		row_of_matrix(const row_of_matrix<Dim>&& rhs)
			: index_(rhs.index_), elements_(rhs.elements_)
		{
		}

		float operator[](size_t j) const
		{
			if (Dim != index_) return elements_[index_ * (Dim+1) + j];
			else if (Dim == j) return 1;
			else return 0;
		}
	};
//}}}
}// namespace math
}// namespace mof
