#ifndef __JMATH__JMATRIXND__
#define __JMATH__JMATRIXND__

#include <cstring>
#include <limits>
#include <cmath>
#include <algorithm>

#include "JIO/JSerialisable.hh"
#include "JLang/JException.hh"
#include "JLang/JEquals.hh"
#include "JLang/JManip.hh"
#include "JMath/JMath.hh"
#include "JMath/JVectorND.hh"

/**
 * \author mdejong
 */

namespace JMATH {}
namespace JPP { using namespace JMATH; }

namespace JMATH {

  using JIO::JReader;
  using JIO::JWriter;
  using JLANG::JEquals;
  using JLANG::JException;
  using JLANG::JNewException;
  using JLANG::JIndexOutOfRange;


  /**
   * Basic NxN matrix.
   */
  struct JMatrixND_t {
    /**
     * Default constructor.
     */
    JMatrixND_t() :
      __p(NULL),
      __n(0),
      __m(0)
    {}


    /**
     * Copy constructor.
     *
     * \param  A        matrix
     */
    JMatrixND_t(const JMatrixND_t& A) :
      JMatrixND_t()
    {
      set(A);
    }


    /**
     * Move constructor.
     *
     * \param  A        matrix
     */
    JMatrixND_t(JMatrixND_t&& A) :
      JMatrixND_t()
    {
      swap(A);
    }


    /**
     * Destructor.
     */
    ~JMatrixND_t()
    {
      clear();
    }


    /**
     * Clear memory.
     */
    void clear()
    {
      if (__p != NULL) {
	delete [] __p;
      }

      __p = NULL;
      __n = 0;
      __m = 0;
    }


    /**
     * Resize matrix.
     *
     * Note that this method does not maintain data in the matrix.
     *
     * \param  size     dimension
     */
    void resize(const size_t size)
    {
      if (size != this->__n) {

	if (size > this->__m) {

	  if (__p != NULL) {
	    delete[] __p;
	  }

	  __m = size;
	  __p = new double[__m*__m];

	  if (__p == NULL) {
	    THROW(JNewException, "JMatrixND::resize(" << size << "): Memory allocation failure.");
	  }
	}

	__n = size;
      }
    }


    /**
     * Set matrix to the null matrix.
     *
     * \return          this matrix
     */
    JMatrixND_t& reset()
    {
      double* p0 = this->data();
      double* p1 = this->data();

      if (!this->empty()) {

	for (size_t i = this->size(); i != 0; --i, ++p1) {
	  *p1 = 0.0;
	}

	for (size_t i = this->size(); i != 1; --i, p1 += this->size()) {
	  memcpy(p1, p0, this->size() * sizeof(double));
	}
      }

      return *this;
    }


    /**
     * Set matrix.
     *
     * \param  A        matrix
     */
    void set(const JMatrixND_t& A)
    {
      this->resize(A.size());

      memcpy(this->data(), A.data(), A.size() * A.size() * sizeof(double));
    }


    /**
     * Swap matrices
     *
     * \param  A        matrix
     */
    void swap(JMatrixND_t& A)
    {
      using std::swap;

      swap(this->__p, A.__p);
      swap(this->__n, A.__n);
      swap(this->__m, A.__m);
    }


    /**
     * Transpose.
     *
     * \return          this matrix
     */
    JMatrixND_t& transpose()
    {
      using std::swap;

      for (size_t row = 0; row != this->size(); ++row) {
        for (size_t col = 0; col != row; ++col) {
          swap((*this)(row,col), (*this)(col,row));
        }
      }

      return *this;
    }


    /**
     * Get dimension of matrix.
     *
     * \return          dimension
     */
    size_t size() const
    {
      return __n;
    }


    /**
     * Get capacity of dimension.
     *
     * \return          capacity
     */
    size_t capacity() const
    {
      return __m;
    }


    /**
     * Check emptyness.
     *
     * \return          true if empty; else false
     */
    bool empty() const
    {
      return __n == 0;
    }


    /**
     * Get pointer to data.
     *
     * \return          pointer to data.
     */
    const double* data() const
    {
      return __p;
    }


    /**
     * Get pointer to data.
     *
     * \return          pointer to data.
     */
    double* data()
    {
      return __p;
    }


    /**
     * Get row data.
     *
     * \param  row      row number
     * \return          pointer to row data
     */
    const double* operator[](size_t row) const
    {
      return __p + row*__n;
    }


    /**
     * Get row data.
     *
     * \param  row      row number
     * \return          pointer to row data
     */
    double* operator[](size_t row)
    {
      return __p + row*__n;
    }


    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double operator()(const size_t row, const size_t col) const
    {
      return *(__p + row*__n + col);
    }


    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double& operator()(const size_t row, const size_t col)
    {
      return *(__p + row*__n + col);
    }
    

    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double at(size_t row, size_t col) const
    {
      if (row >= this->size() ||
	  col >= this->size()) {
	THROW(JIndexOutOfRange, "JMatrixND::at(" << row << "," << col << "): index out of range.");
      }

      return (*this)(row, col);
    }


    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double& at(size_t row, size_t col)
    {
      if (row >= this->size() ||
	  col >= this->size()) {
	THROW(JIndexOutOfRange, "JMatrixND::at(" << row << "," << col << "): index out of range.");
      }

      return (*this)(row, col);
    }


  protected:
    double* __p;    //!< pointer to data
    size_t  __n;    //!< dimension of matrix
    size_t  __m;    //!< capacity  of matrix
  };


  /**
   * NxN matrix.
   */
  struct JMatrixND :
    public JMatrixND_t,
    public JMath  <JMatrixND>,
    public JEquals<JMatrixND>
  {
  protected:
    JMatrixND_t ws;

    /**
     * Get work space.
     *
     * \return          work space
     */
    JMatrixND_t& getInstance()
    {
      return ws;
    }

  public:

    using JMath<JMatrixND>::mul;

    /**
     * Default constructor.
     */
    JMatrixND()
    {}


    /**
     * Constructor.
     *
     * The matrix is set to zero.
     *
     * \param  size     dimension
     */
    JMatrixND(const size_t size)
    {
      resize(size);
      reset();
    }


    /**
     * Copy constructor.
     *
     * \param  A        matrix
     */
    JMatrixND(const JMatrixND& A)
    {
      set(A);
    }


    /**
     * Move constructor.
     *
     * \param  A        matrix
     */
    JMatrixND(JMatrixND&& A)
    {
      swap(A);
    }


    /**
     * Assignment operator.
     *
     * \param  A        matrix
     */
    JMatrixND& operator=(const JMatrixND& A)
    {
      this->set(A);

      return *this;
    }


    /**
     * Move assignment operator.
     *
     * \param  A        matrix
     */
    JMatrixND& operator=(JMatrixND&& A)
    {
      this->swap(A);

      return *this;
    }


    /**
     * Resize matrix.
     *
     * Note that this method does not maintain data in the matrix.
     *
     * \param  size     dimension
     */
    void resize(const size_t size)
    {
      static_cast<JMatrixND_t&>(*this).resize(size);

      getInstance().resize(size);
    }


    /**
     * Set matrix to the null matrix.
     *
     * \return          this matrix
     */
    JMatrixND& reset()
    {
      JMatrixND_t::reset();

      JMatrixND_t& A = getInstance();

      A.resize(this->size());

      return *this;
    }


    /**
     * Set to identity matrix
     *
     * \return          this matrix
     */
    JMatrixND& setIdentity()
    {
      reset();

      for (size_t i = 0; i != this->size(); ++i) {
        (*this)(i,i) = 1.0;
      }

      return *this;
    }


    /**
     * Negate matrix.
     *
     * \return          this matrix
     */
    JMatrixND& negate()
    {
      double* p = this->data();

      for (size_t i = this->size()*this->size(); i != 0; --i, ++p) {
	*p = -*p;
      }

      return *this;
    }


    /**
     * Matrix addition.
     *
     * \param  A        matrix
     * \return          this matrix
     */
    JMatrixND& add(const JMatrixND& A)
    {
      if (this->size() == A.size()) {

	double*       p = this->data();
	const double* q = A.data();

	for (size_t i = this->size()*this->size(); i != 0; --i, ++p, ++q) {
	  *p += *q;
	}

      } else {
        THROW(JException, "JMatrixND::add() inconsistent matrix dimensions " << this->size() << ' ' << A.size());
      }

      return *this;
    }


    /**
     * Matrix subtraction.
     *
     * \param  A        matrix
     * \return          this matrix
     */
    JMatrixND& sub(const JMatrixND& A)
    {
      if (this->size() == A.size()) {

	double*       p = this->data();
	const double* q = A.data();

	for (size_t i = this->size()*this->size(); i != 0; --i, ++p, ++q) {
	  *p -= *q;
	}

      } else {
        THROW(JException, "JMatrixND::sub() inconsistent matrix dimensions " << this->size() << ' ' << A.size());
      }

      return *this;
    }


    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix
     */
    JMatrixND& mul(const double factor)
    {
      double* p = this->data();

      for (size_t i = this->size()*this->size(); i != 0; --i, ++p) {
	*p *= factor;
      }

      return *this;
    }


    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix
     */
    JMatrixND& div(const double factor)
    {
      double* p = this->data();

      for (size_t i = this->size()*this->size(); i != 0; --i, ++p) {
	*p /= factor;
      }

      return *this;
    }


    /**
     * Matrix multiplication.
     *
     * \param  A        matrix
     * \param  B        matrix
     * \return          this matrix
     */
    JMatrixND& mul(const JMatrixND& A,
                   const JMatrixND& B)
    {
      if (A.size() == B.size()) {

        this->resize(A.size());

	if (!this->empty()) {

	  JMatrixND_t& C = getInstance();
	  
	  C.set(B);
	  C.transpose();

	  size_t i, row;

	  for (row = 0; row + 4 <= this->size(); row += 4) {              // process rows by four

	    double* p0 = (*this)[row + 0];
	    double* p1 = (*this)[row + 1];
	    double* p2 = (*this)[row + 2];
	    double* p3 = (*this)[row + 3];

	    for (size_t col = 0; col != this->size(); ++col, ++p0, ++p1, ++p2, ++p3) {

	      double w0 = 0.0;
	      double w1 = 0.0;
	      double w2 = 0.0;
	      double w3 = 0.0;

	      const double* a0 =  A[row + 0];
	      const double* a1 =  A[row + 1];
	      const double* a2 =  A[row + 2];
	      const double* a3 =  A[row + 3];

	      const double* c0 =  C[col];

	      for (i = 0; i + 4 <= this->size(); i += 4, a0 += 4, a1 += 4, a2 += 4, a3 += 4, c0 += 4) {
		w0  +=  a0[0] * c0[0]  +  a0[1] * c0[1]  +  a0[2] * c0[2]  +  a0[3] * c0[3];
		w1  +=  a1[0] * c0[0]  +  a1[1] * c0[1]  +  a1[2] * c0[2]  +  a1[3] * c0[3];
		w2  +=  a2[0] * c0[0]  +  a2[1] * c0[1]  +  a2[2] * c0[2]  +  a2[3] * c0[3];
		w3  +=  a3[0] * c0[0]  +  a3[1] * c0[1]  +  a3[2] * c0[2]  +  a3[3] * c0[3];
	      }

	      for ( ; i != this->size(); ++i, ++a0, ++a1, ++a2, ++a3, ++c0) {
		w0  +=  (*a0) * (*c0);
		w1  +=  (*a1) * (*c0);
		w2  +=  (*a2) * (*c0);
		w3  +=  (*a3) * (*c0);
	      }

	      *p0 = w0;
	      *p1 = w1;
	      *p2 = w2;
	      *p3 = w3;
	    }
	  }

	  for ( ; row != this->size(); ++row) {                  // remaining rows

	    double* p0 = (*this)[row + 0];

	    for (size_t col = 0; col != this->size(); ++col, ++p0) {

	      double w0 = 0.0;

	      const double* a0 =  A[row + 0];
	      const double* c0 =  C[col];

	      for (i = 0; i + 4 <= this->size(); i += 4, a0 += 4, c0 += 4) {
		w0  +=  a0[0] * c0[0]  +  a0[1] * c0[1]  +  a0[2] * c0[2]  +  a0[3] * c0[3];
	      }

	      for ( ; i != this->size(); ++i, ++a0, ++c0) {
		w0  +=  (*a0) * (*c0);
	      }

	      *p0 = w0;
	    }
	  }
	}

      } else {
        THROW(JException, "JMatrixND::mul() inconsistent matrix dimensions " << A.size() << ' ' << B.size());
      }

      return *this;
    }


    /**
     * Equality.
     *
     * \param  A             matrix
     * \param  eps           numerical precision
     * \return               true if matrices identical; else false
     */
    bool equals(const JMatrixND& A,
                const double     eps = std::numeric_limits<double>::min()) const
    {
      if (this->size() == A.size()) {

	for (size_t row = 0; row != this->size(); ++row) {

	  const double* p = (*this)[row];
	  const double* q = A      [row];

	  for (size_t i = this->size(); i != 0; --i, ++p, ++q) {
	    if (fabs(*p - *q) > eps) {
	      return false;
	    }
	  }
	}

        return true;

      } else {
        THROW(JException, "JMatrixND::equals() inconsistent matrix dimensions " << this->size() << ' ' << A.size());
      }
    }


    /**
     * Test identity.
     *
     * \param  eps           numerical precision
     * \return               true if identity matrix; else false
     */
    bool isIdentity(const double eps = std::numeric_limits<double>::min()) const
    {
      for (size_t i = 0; i != this->size(); ++i) {

        if (fabs(1.0 - (*this)(i,i)) > eps) {
          return false;
        };

        for (size_t j = 0; j != i; ++j) {
          if (fabs((*this)(i,j)) > eps || fabs((*this)(j,i)) > eps) {
            return false;
          };
        }
      }

      return true;
    }


    /**
     * Test symmetry.
     *
     * \param  eps           numerical precision
     * \return               true if symmetric matrix; else false
     */
    bool isSymmetric(const double eps = std::numeric_limits<double>::min()) const
    {
      for (size_t i = 0; i != this->size(); ++i) {
        for (size_t j = 0; j != i; ++j) {
          if (fabs((*this)(i,j) - (*this)(j,i)) > eps) {
            return false;
          };
        }
      }

      return true;
    }


    /**
     * Get dot product.
     *
     * The dot product corresponds to \f[ v^{T} \times A \times v \f].
     *
     * \param  v             vector
     * \return               dot product
     */
    double getDot(const JVectorND& v) const
    {
      double dot = 0.0;

      for (size_t i = 0; i != v.size(); ++i) {

	const double* p = (*this)[i];

	double w = 0.0;

	for (JVectorND::const_iterator y = v.begin(); y != v.end(); ++y, ++p) {
	  w += (*p) * (*y);
	}

	dot += v[i] * w;
      }

      return dot;
    }


    /**
     * Read matrix from input.
     *
     * \param  in       reader
     * \param  A        matrix
     * \return          reader
     */
    friend inline JReader& operator>>(JReader& in, JMatrixND& A)
    {
      size_t size;

      in >> size;

      A.resize(size);

      size_t n = A.size() * A.size();      

      for (double* p = A.data(); n != 0; --n, ++p) {
	in >> *p;
      }

      return in;
    }


    /**
     * Write matrix to output.
     *
     * \param  out      writer
     * \param  A        matrix
     * \return          writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JMatrixND& A)
    {
      out << A.size();

      size_t n = A.size() * A.size();

      for (const double* p = A.data(); n != 0; --n, ++p) {
	out << *p;
      }

      return out;
    }


    /**
     * Print ASCII formatted output.
     *
     * \param  out      output stream
     * \param  A        matrix
     * \return          output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JMatrixND& A)
    {
      using namespace std;

      const JFormat format(out, getFormat<JMatrixND>(JFormat_t(10, 3, std::ios::fixed | std::ios::showpos)));

      for (size_t row = 0; row != A.size(); ++row) {

	for (size_t col = 0; col != A.size(); ++col) {
          out << format << A(row,col) << ' ';
        }

        out << endl;
      }

      return out;
    }


  protected:
    /*
     * Swap rows.
     *
     * \param  r1            row
     * \param  r2            row
     */
    void rswap(size_t r1, size_t r2)
    {
      JMatrixND_t& A = getInstance();

      A.resize(this->size());
      
      memcpy(A.data(),    (*this)[r1], this->size() * sizeof(double));
      memcpy((*this)[r1], (*this)[r2], this->size() * sizeof(double));
      memcpy((*this)[r2], A.data(),    this->size() * sizeof(double));
    }


    /**
     * Swap columns.
     *
     * \param  c1            column
     * \param  c2            column
     */
    void cswap(size_t c1, size_t c2)
    {
      using std::swap;

      double* p1 = this->data() + c1;
      double* p2 = this->data() + c2;

      for (size_t i = this->size(); i != 0; --i, p1 += this->size(), p2 += this->size()) {
	swap(*p1, *p2);
      }
    }
  };
}

#endif