#include <iostream>
#include <iomanip>			// setw



#ifndef _tvmet_restrict
#define _tvmet_restrict  __restrict__
#endif


namespace tvmet {

/* forwards */
template<class T, std::size_t Rows, std::size_t Cols> class Matrix;


/**
 * short type promotion
 */
template<class T1, class T2>
struct PromoteTraits { };

template<>
struct PromoteTraits<double, double> {
    typedef double value_type;
};


/**
 * \class XprNull Null.h "tvmet/xpr/Null.h"
 * \brief Null object design pattern
 */
class XprNull
{
  XprNull& operator=(const XprNull&);

public:
  explicit XprNull() { }
};


#define TVMET_BINARY_OPERATOR(OP)                   			\
template< class T >                    					\
inline                     						\
T operator OP (const T& lhs, XprNull) { return lhs; }

TVMET_BINARY_OPERATOR(+)
TVMET_BINARY_OPERATOR(*)
#undef TVMET_BINARY_OPERATOR


namespace meta {


/**
 * \class Matrix Matrix.h "tvmet/meta/Matrix.h"
 * \brief Meta %Matrix class using expression templates
 */
template<std::size_t Rows, std::size_t Cols,
	 std::size_t RowStride=0, std::size_t ColStride=0>
class Matrix
{
  Matrix();
  Matrix(const Matrix&);
  Matrix& operator=(const Matrix&);

private:
  enum {
    doRows = (RowStride < Rows - 1) ? 1 : 0,	/**< recursive counter Rows. */
    doCols = (ColStride < Cols - 1) ? 1 : 0	/**< recursive counter Cols. */
  };

public:
  /** Assign an expression expr on columns on given row using the functional fn. */
  template<class Mtrx, class E, class Fcnl>
  static inline
  void assign2(Mtrx& mat, const E& expr, const Fcnl& fn) {
    fn.applyOn(mat(RowStride, ColStride), expr(RowStride, ColStride));
    Matrix<Rows * doCols, Cols * doCols,
           RowStride * doCols, (ColStride+1) * doCols>::assign2(mat, expr, fn);
  }

  /** Assign an expression expr on row-wise using the functional fn. */
  template<class Mtrx, class E, class Fcnl>
  static inline
  void assign(Mtrx& mat, const E& expr, const Fcnl& fn) {
    Matrix<Rows, Cols,
           RowStride, 0>::assign2(mat, expr, fn);
    Matrix<Rows * doRows, Cols * doRows,
          (RowStride+1) * doRows, 0>::assign(mat, expr, fn);
  }
};


/**
 * \class Matrix<0, 0, 0, 0> Matrix.h "tvmet/meta/Matrix.h"
 * \brief Meta %Matrix specialized for recursion
 */
template<>
class Matrix<0, 0, 0, 0>
{
  Matrix();
  Matrix(const Matrix&);
  Matrix& operator=(const Matrix&);

public:
  template<class Mtrx, class E, class Fcnl>
  static inline void assign2(Mtrx&, const E&, const Fcnl&) { }

  template<class Mtrx, class E, class Fcnl>
  static inline void assign(Mtrx&, const E&, const Fcnl&) { }
};



/**
 * \class gemm Gemm.h "tvmet/meta/Gemm.h"
 * \brief Meta class for matrix-matrix operations, like product.
 * \note The rows of matrix 2 have to be equal to cols of matrix 1, square
 *       matrix is mandatory.
 */
template<std::size_t Rows1, std::size_t Cols1,
	 std::size_t Cols2,
	 std::size_t RowStride1, std::size_t ColStride1,
	 std::size_t RowStride2, std::size_t ColStride2,
	 std::size_t K>
class gemm
{
  gemm();
  gemm(const gemm&);
  gemm& operator=(const gemm&);

private:
  enum {
    doIt = (K != Cols1 - 1) 		/**< recursive counter */
  };

public:
  template<class T1, class T2>
  static inline
  typename PromoteTraits<T1, T2>::value_type
  prod(const T1* _tvmet_restrict lhs, const T2* _tvmet_restrict rhs, std::size_t i, std::size_t j) {
    return lhs[i * RowStride1 + K * ColStride1] * rhs[K * RowStride2 + j * ColStride2]
      + gemm<Rows1 * doIt, Cols1 * doIt,
               Cols2 * doIt, RowStride1 * doIt, ColStride1 * doIt,
               RowStride2 * doIt, ColStride2 * doIt, (K+1) * doIt>::prod(lhs, rhs, i, j);
  }
};


/**
 * \class gemm<0,0,0,0,0,0,0,0> Gemm.h "tvmet/meta/Gemm.h"
 * \brief gemm Specialized for recursion.
 */
template<>
class gemm<0,0,0,0,0,0,0,0>
{
  gemm();
  gemm(const gemm&);
  gemm& operator=(const gemm&);

public:
  static inline
  XprNull prod(const void*, const void*, std::size_t, std::size_t) {
    return XprNull();
  }
};


} // namespace meta


/**
 * \class fcnl_Assign BinaryFunctionals.h "tvmet/BinaryFunctionals.h"
 * \brief Binary operator for assign operations.
 */
template <class T1, class T2>
struct fcnl_Assign {
  typedef void						return_type;
  static inline return_type applyOn(T1& _tvmet_restrict lhs, T2 rhs) {
    lhs = static_cast<T1>(rhs);
  }
};


/**
 * \class XprMMProduct MMProduct.h "tvmet/xpr/MMProduct.h"
 * \brief Expression for matrix-matrix product.
 * \note The Rows2 has to be  equal to Cols1.
 */
template<class T1, std::size_t Rows1, std::size_t Cols1,
	 class T2, std::size_t Cols2,
	 std::size_t RowStride1, std::size_t ColStride1,
	 std::size_t RowStride2, std::size_t ColStride2>
class XprMMProduct
{
private:
  XprMMProduct();
  XprMMProduct& operator=(const XprMMProduct&);

public:
  typedef typename PromoteTraits<T1, T2>::value_type	value_type;

public:
  /** Constructor. */
  explicit XprMMProduct(const T1* _tvmet_restrict lhs, const T2* _tvmet_restrict rhs)
    : m_lhs(lhs), m_rhs(rhs)
  { }

  /** index operator for arrays/matrices */
  value_type operator()(std::size_t i, std::size_t j) const {
    return meta::gemm<Rows1, Cols1,
                      Cols2,
                      RowStride1, ColStride1,
                      RowStride2, ColStride2, 0>::prod(m_lhs, m_rhs, i, j);
  }

private:
  const T1* _tvmet_restrict 				m_lhs;
  const T2* _tvmet_restrict 				m_rhs;
};


/**
 * \class XprMatrixTranspose MatrixTranspose.h "tvmet/xpr/MatrixTranspose.h"
 * \brief Expression for transpose matrix
 */
template<class E>
class XprMatrixTranspose
{
  XprMatrixTranspose();
  XprMatrixTranspose& operator=(const XprMatrixTranspose&);

public:
  typedef E						expr_type;
  typedef typename expr_type::value_type		value_type;

public:
  /** Constructor. */
  explicit XprMatrixTranspose(const expr_type& e)
    : m_expr(e)
  { }

  /** index operator for arrays/matrices. This simple swap the index
      access for transpose. */
  value_type operator()(std::size_t i, std::size_t j) const { return m_expr(j, i); }

private:
  const expr_type& _tvmet_restrict			m_expr;
};


/**
 * \class XprMatrix Matrix.h "tvmet/xpr/Matrix.h"
 * \brief Represents the expression for vectors at any node in the parse tree.
 */
template<class E, std::size_t Rows, std::size_t Cols>
class XprMatrix
{
  XprMatrix();
  XprMatrix& operator=(const XprMatrix&);

public:
  typedef E						expr_type;
  typedef typename expr_type::value_type		value_type;

public:
  /** Constructor. */
  explicit XprMatrix(const expr_type& e)
    : m_expr(e)
  { }

  /** index operator for arrays/matrizes */
  value_type operator()(std::size_t i, std::size_t j) const {
    return m_expr(i, j);
  }

private:
  const expr_type& _tvmet_restrict			m_expr;
};


/**
 * \class MatrixConstReference Matrix.h "tvmet/Matrix.h"
 * \brief value iterator for ET
 */
template<class T,
	 std::size_t Rows, std::size_t Cols,
	 std::size_t RowStride=Cols, std::size_t ColStride=1>
class MatrixConstReference
{
public: // types
  typedef T						value_type;

private:
  MatrixConstReference();
  MatrixConstReference& operator=(const MatrixConstReference&);

public:
  /** Constructor. */
  explicit MatrixConstReference(const Matrix<T, Rows, Cols>& rhs)
    : m_data(rhs.data())
  { }

public: // access operators
  /** access by index. */
  value_type operator()(std::size_t i, std::size_t j) const {
    return m_data[i * RowStride + j * ColStride];
  }

private:
  const value_type* _tvmet_restrict 			m_data;
};


/**
 * \class Matrix Matrix.h "tvmet/Matrix.h"
 * \brief A tiny matrix class.
 */
template<class T, std::size_t Rows, std::size_t Cols>
class Matrix
{
public:
  /** Data type of the tvmet::Matrix. */
  typedef T						value_type;

  /** This type of tvmet::Matrix, used to simplify member wrote. */
  typedef Matrix<T, Rows, Cols>				this_type;

public:
  /** Default Constructor. The allocated memory region isn't cleared. If you want
   a clean use the constructor argument zero. */
  explicit Matrix() { }

  /** Construct a matrix by expression. */
  template<class E>
  explicit Matrix(const XprMatrix<E, Rows, Cols>& expr) {
    assign(expr, fcnl_Assign<value_type, typename E::value_type>());
  }

public: // access operators
  value_type* _tvmet_restrict data() { return m_data; }
  const value_type* _tvmet_restrict data() const { return m_data; }

public: // index access operators
  value_type& _tvmet_restrict operator()(std::size_t i, std::size_t j) {
    return m_data[i * Cols + j];
  }

  value_type operator()(std::size_t i, std::size_t j) const {
    return m_data[i * Cols + j];
  }

public: // ET interface
  typedef MatrixConstReference<T, Rows, Cols>   	ConstReference;

  /** Return a const Reference of the internal data */
  ConstReference const_ref() const { return ConstReference(*this); }

private: // assign operations
  /** Assign an expression to this matrix using the functional fn. */
  template<class E, class Fcnl>
  void assign(const E& expr, const Fcnl& fn) {
    meta::Matrix<Rows, Cols>::assign(*this, expr, fn);
  }

public:  // assign operations
  /** Assign a given XprMatrix element wise to this matrix. */
  template <class E> this_type& operator=(const XprMatrix<E, Rows, Cols>& rhs) {
    this->assign(rhs, fcnl_Assign<value_type, typename E::value_type>());
    return *this;
  }

public: // io
  std::ostream& print_on(std::ostream& os) const;

private:
  value_type 						m_data[Rows * Cols];
};


/*
 * member function implementation for i/o
 */
template<class T, std::size_t Rows, std::size_t Cols>
inline
std::ostream& Matrix<T, Rows, Cols>::print_on(std::ostream& os) const
{
  std::streamsize w = os.width();


    os << std::setw(0) << "Matrix<" << typeid(T).name() << ", "
       << Rows << ", " << Cols << "> = [\n";
    for(std::size_t i = 0; i < Rows; ++i) {
      os << " [";
      for(std::size_t j = 0; j < (Cols - 1); ++j) {
	os << std::setw(w) << this->operator()(i, j) << ", ";
      }
      os << std::setw(w) << this->operator()(i, Cols - 1)
	 << (i != (Rows-1) ? "],\n" : "]\n");
    }
    os << "]";

  return os;
}


/**
 * \fn operator<<(std::ostream& os, const Matrix<T, Rows, Cols>& rhs)
 * \brief Overload operator for i/o
 */
template<class T, std::size_t Rows, std::size_t Cols>
inline
std::ostream& operator<<(std::ostream& os, const Matrix<T, Rows, Cols>& rhs) {
  return rhs.print_on(os);
}


/**
 * \fn prod(const XprMatrix<E1, Rows1, Cols1>& lhs, const Matrix<T2, Cols1, Cols2>& rhs)
 * \brief Evaluate the product of XprMatrix and Matrix.
 * \note This function is using temporaries for pre-evaluation of the matrix1
 *       expression.
 */
template<class E1, std::size_t Rows1, std::size_t Cols1,
	 class T2, std::size_t Cols2>
inline
XprMatrix<
  XprMMProduct<
    typename E1::value_type, Rows1, Cols1,
    T2, Cols2,
    Cols1, 1,
    Cols2, 1
  >,
  Rows1, Cols2
>
prod(const XprMatrix<E1, Rows1, Cols1>& lhs, const Matrix<T2, Cols1, Cols2>& rhs) {
  typedef Matrix<typename E1::value_type, Rows1, Cols1>	temp_matrix_type;
  typedef XprMMProduct<
    typename E1::value_type, Rows1, Cols1,
    T2, Cols2,
    Cols1, 1,
    Cols2, 1
  >							expr_type;
#if defined(TVMET_RVO_BUG_WO)
  temp_matrix_type 					temp_lhs(lhs);
  return XprMatrix<expr_type, Rows1, Cols2>(expr_type(temp_lhs.data(), rhs.data()));
#else
  return XprMatrix<expr_type, Rows1, Cols2>(expr_type(temp_matrix_type(lhs).data(), rhs.data()));
#endif
}


/**
 * \fn trans(const Matrix<T, Rows, Cols>& rhs)
 * \brief Transpose the matrix
 */
template<class T, std::size_t Rows, std::size_t Cols>
inline
XprMatrix<
  XprMatrixTranspose<
    MatrixConstReference<T, Rows, Cols>
  >,
  Cols, Rows
>
trans(const Matrix<T, Rows, Cols>& rhs) {
  typedef XprMatrixTranspose<
    MatrixConstReference<T, Rows, Cols>
  >							expr_type;
  return XprMatrix<expr_type, Cols, Rows>(expr_type(rhs.const_ref()));
}

} // namespace tvmet


/**
 * Test driver
 */
using namespace std;

int main()
{
  tvmet::Matrix<double,3,2>	B;
  tvmet::Matrix<double,3,3>	D;
  tvmet::Matrix<double,2,2>	K;

  B(0,0) = -0.05;	B(0,1) =  0;
  B(1,0) =  0;		B(1,1) =  0.05;
  B(2,0) =  0.05;	B(2,1) = -0.05;

  D(0,0) = 2000;	D(0,1) = 1000;		D(0,2) = 0;
  D(1,0) = 1000;	D(1,1) = 2000;		D(1,2) = 0;
  D(2,0) = 0;		D(2,1) = 0;		D(2,2) = 500;

  cout << "B = " << B << endl;
  cout << "D = " << D << endl;

  K = prod(prod(trans(B), D), B);
  cout << "K = " << K << endl;
}