#ifndef __JMATRIX5D__
#define __JMATRIX5D__

#include <limits>
#include <cmath>
#include <ostream>
#include <iomanip>

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


/**
 * \author mdejong
 */

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

namespace JMATH {

  using JIO::JReader;
  using JIO::JWriter;
  using JLANG::JEquals;


  /**
   * 5 x 5 matrix
   */
  class JMatrix5D :
    public JMath  <JMatrix5D>,
    public JEquals<JMatrix5D>
  {
  public:

    using JMath<JMatrix5D>::mul;


    /**
     * Default constructor.
     */
    JMatrix5D() :
      a00(0.0), a01(0.0), a02(0.0), a03(0.0), a04(0.0),
      a10(0.0), a11(0.0), a12(0.0), a13(0.0), a14(0.0),
      a20(0.0), a21(0.0), a22(0.0), a23(0.0), a24(0.0),
      a30(0.0), a31(0.0), a32(0.0), a33(0.0), a34(0.0),
      a40(0.0), a41(0.0), a42(0.0), a43(0.0), a44(0.0)
    {}


    /**
     * Contructor.
     * 
     * \param   __a00   (0,0)
     * \param   __a01   (0,1)
     * \param   __a02   (0,2)
     * \param   __a03   (0,3)
     * \param   __a04   (0,4)
     * \param   __a10   (1,0)
     * \param   __a11   (1,1)
     * \param   __a12   (1,2)
     * \param   __a13   (1,3)
     * \param   __a14   (1,4)
     * \param   __a20   (2,0)
     * \param   __a21   (2,1)
     * \param   __a22   (2,2)
     * \param   __a23   (2,3)
     * \param   __a24   (2,4)
     * \param   __a30   (3,0)
     * \param   __a31   (3,1)
     * \param   __a32   (3,2)
     * \param   __a33   (3,3)
     * \param   __a34   (3,4)
     * \param   __a40   (4,0)
     * \param   __a41   (4,1)
     * \param   __a42   (4,2)
     * \param   __a43   (4,3)
     * \param   __a44   (4,4)
     */
    JMatrix5D(const double __a00, const double __a01, const double __a02, const double __a03, const double __a04,
	      const double __a10, const double __a11, const double __a12, const double __a13, const double __a14,
	      const double __a20, const double __a21, const double __a22, const double __a23, const double __a24,
	      const double __a30, const double __a31, const double __a32, const double __a33, const double __a34,
	      const double __a40, const double __a41, const double __a42, const double __a43, const double __a44) :
      a00(__a00), a01(__a01), a02(__a02), a03(__a03), a04(__a04),
      a10(__a10), a11(__a11), a12(__a12), a13(__a13), a14(__a14),
      a20(__a20), a21(__a21), a22(__a22), a23(__a23), a24(__a24),
      a30(__a30), a31(__a31), a32(__a32), a33(__a33), a34(__a34),
      a40(__a40), a41(__a41), a42(__a42), a43(__a43), a44(__a44)
    {}


    /**
     * Get reference to unique instance of this class object.
     *
     * \return          zero matrix
     */
    static const JMatrix5D& getInstance()
    {
      static JMatrix5D matrix;

      return matrix;
    }


    /**
     * Set to identity matrix
     *
     * \return          this matrix
     */
    JMatrix5D& setIdentity()
    {
      a00 = 1.0;  a01 = 0.0;  a02 = 0.0;  a03 = 0.0;  a04 = 0.0;  
      a10 = 0.0;  a11 = 1.0;  a12 = 0.0;  a13 = 0.0;  a14 = 0.0;  
      a20 = 0.0;  a21 = 0.0;  a22 = 1.0;  a23 = 0.0;  a24 = 0.0;  
      a30 = 0.0;  a31 = 0.0;  a32 = 0.0;  a33 = 1.0;  a34 = 0.0;  
      a40 = 0.0;  a41 = 0.0;  a42 = 0.0;  a43 = 0.0;  a44 = 1.0;  

      return *this;
    }


    /**
     * Get reference to unique instance of this class object.
     *
     * \return          identity matrix
     */
    static const JMatrix5D& getIdentity()
    {
      static JMatrix5D matrix(JMatrix5D().setIdentity());

      return matrix;
    }


    /**
     * Set matrix.
     *
     * \param  A        matrix
     */
    void set(const JMatrix5D& A)
    {
      static_cast<JMatrix5D&>(*this) = A;
    }


    /**
     * Set matrix to the null matrix.
     *
     * \return          this matrix
     */
    JMatrix5D& reset()
    {
      *this = JMatrix5D();

      return *this;
    }


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

      swap(a10, a01);
      swap(a20, a02);
      swap(a21, a12);
      swap(a30, a03);
      swap(a31, a13);
      swap(a32, a23);
      swap(a40, a04);
      swap(a41, a14);
      swap(a42, a24);
      swap(a43, a34);

      return *this;
    }


    /**
     * Negate matrix.
     *
     * \return          -this matrix
     */
    JMatrix5D& negate()
    {
      a00 = -a00;  a01 = -a01;  a02 = -a02;  a03 = -a03;  a04 = -a04;
      a10 = -a10;  a11 = -a11;  a12 = -a12;  a13 = -a13;  a14 = -a14;
      a20 = -a20;  a21 = -a21;  a22 = -a22;  a13 = -a23;  a14 = -a24;
      a30 = -a30;  a31 = -a31;  a32 = -a32;  a33 = -a33;  a34 = -a34;
      a40 = -a40;  a41 = -a41;  a42 = -a42;  a43 = -a43;  a44 = -a44;

      return *this;
    }


    /**
     * Matrix addition.
     *
     * \param  A        matrix
     * \return          this matrix + A
     */
    JMatrix5D& add(const JMatrix5D& A)
    {
      a00 += A.a00;  a01 += A.a01;  a02 += A.a02;  a03 += A.a03;  a04 += A.a04;
      a10 += A.a10;  a11 += A.a11;  a12 += A.a12;  a13 += A.a13;  a14 += A.a14;
      a20 += A.a20;  a21 += A.a21;  a22 += A.a22;  a23 += A.a23;  a24 += A.a24;
      a30 += A.a30;  a31 += A.a31;  a32 += A.a32;  a33 += A.a33;  a34 += A.a34;
      a40 += A.a40;  a41 += A.a41;  a42 += A.a42;  a43 += A.a43;  a44 += A.a44;
  
      return *this;
    }


    /**
     * Matrix subtraction.
     *
     * \param  A        matrix
     * \return          this matrix - A
     */
    JMatrix5D& sub(const JMatrix5D& A)
    {
      a00 -= A.a00;  a01 -= A.a01;  a02 -= A.a02;  a03 -= A.a03;  a04 -= A.a04;
      a10 -= A.a10;  a11 -= A.a11;  a12 -= A.a12;  a13 -= A.a13;  a14 -= A.a14;
      a20 -= A.a20;  a21 -= A.a21;  a22 -= A.a22;  a23 -= A.a23;  a24 -= A.a24;
      a30 -= A.a30;  a31 -= A.a31;  a32 -= A.a32;  a33 -= A.a33;  a34 -= A.a34;
      a40 -= A.a40;  a41 -= A.a41;  a42 -= A.a42;  a43 -= A.a43;  a44 -= A.a44;
  
      return *this;
    }

    
    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix * factor
     */
    JMatrix5D& mul(const double factor)
    {
      a00 *= factor; a01 *= factor; a02 *= factor; a03 *= factor; a04 *= factor;
      a10 *= factor; a11 *= factor; a12 *= factor; a13 *= factor; a14 *= factor;
      a20 *= factor; a21 *= factor; a22 *= factor; a23 *= factor; a24 *= factor;
      a30 *= factor; a31 *= factor; a32 *= factor; a33 *= factor; a34 *= factor;
      a40 *= factor; a41 *= factor; a42 *= factor; a43 *= factor; a44 *= factor;

      return *this;
    }


    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix / factor
     */
    JMatrix5D& div(const double factor)
    {
      a00 /= factor; a01 /= factor; a02 /= factor; a03 /= factor; a04 /= factor;
      a10 /= factor; a11 /= factor; a12 /= factor; a13 /= factor; a14 /= factor;
      a20 /= factor; a21 /= factor; a22 /= factor; a23 /= factor; a24 /= factor;
      a30 /= factor; a31 /= factor; a32 /= factor; a33 /= factor; a34 /= factor;
      a40 /= factor; a41 /= factor; a42 /= factor; a43 /= factor; a44 /= factor;

      return *this;
    }


    /**
     * Matrix multiplication.
     *
     * \param  A        matrix
     * \param  B        matrix
     * \return          this matrix
     */
    const JMatrix5D& mul(const JMatrix5D& A, 
			 const JMatrix5D& B)
    {
      a00 = A.a00 * B.a00  +  A.a01 * B.a10  +  A.a02 * B.a20  +  A.a03 * B.a30  +  A.a04 * B.a40;
      a01 = A.a00 * B.a01  +  A.a01 * B.a11  +  A.a02 * B.a21  +  A.a03 * B.a31  +  A.a04 * B.a41;
      a02 = A.a00 * B.a02  +  A.a01 * B.a12  +  A.a02 * B.a22  +  A.a03 * B.a32  +  A.a04 * B.a42;
      a03 = A.a00 * B.a03  +  A.a01 * B.a13  +  A.a02 * B.a23  +  A.a03 * B.a33  +  A.a04 * B.a43;
      a04 = A.a00 * B.a04  +  A.a01 * B.a14  +  A.a02 * B.a24  +  A.a03 * B.a34  +  A.a04 * B.a44;
	
      a10 = A.a10 * B.a00  +  A.a11 * B.a10  +  A.a12 * B.a20  +  A.a13 * B.a30  +  A.a14 * B.a40;
      a11 = A.a10 * B.a01  +  A.a11 * B.a11  +  A.a12 * B.a21  +  A.a13 * B.a31  +  A.a14 * B.a41;
      a12 = A.a10 * B.a02  +  A.a11 * B.a12  +  A.a12 * B.a22  +  A.a13 * B.a32  +  A.a14 * B.a42;
      a13 = A.a10 * B.a03  +  A.a11 * B.a13  +  A.a12 * B.a23  +  A.a13 * B.a33  +  A.a14 * B.a43;
      a14 = A.a10 * B.a04  +  A.a11 * B.a14  +  A.a12 * B.a24  +  A.a13 * B.a34  +  A.a14 * B.a44;
      
      a20 = A.a20 * B.a00  +  A.a21 * B.a10  +  A.a22 * B.a20  +  A.a23 * B.a30  +  A.a24 * B.a40;
      a21 = A.a20 * B.a01  +  A.a21 * B.a11  +  A.a22 * B.a21  +  A.a23 * B.a31  +  A.a24 * B.a41;
      a22 = A.a20 * B.a02  +  A.a21 * B.a12  +  A.a22 * B.a22  +  A.a23 * B.a32  +  A.a24 * B.a42;
      a23 = A.a20 * B.a03  +  A.a21 * B.a13  +  A.a22 * B.a23  +  A.a23 * B.a33  +  A.a24 * B.a43;
      a24 = A.a20 * B.a04  +  A.a21 * B.a14  +  A.a22 * B.a24  +  A.a23 * B.a34  +  A.a24 * B.a44;
	
      a30 = A.a30 * B.a00  +  A.a31 * B.a10  +  A.a32 * B.a20  +  A.a33 * B.a30  +  A.a34 * B.a40;
      a31 = A.a30 * B.a01  +  A.a31 * B.a11  +  A.a32 * B.a21  +  A.a33 * B.a31  +  A.a34 * B.a41;
      a32 = A.a30 * B.a02  +  A.a31 * B.a12  +  A.a32 * B.a22  +  A.a33 * B.a32  +  A.a34 * B.a42;
      a33 = A.a30 * B.a03  +  A.a31 * B.a13  +  A.a32 * B.a23  +  A.a33 * B.a33  +  A.a34 * B.a43;
      a34 = A.a30 * B.a04  +  A.a31 * B.a14  +  A.a32 * B.a24  +  A.a33 * B.a34  +  A.a34 * B.a44;
      
      a40 = A.a40 * B.a00  +  A.a41 * B.a10  +  A.a42 * B.a20  +  A.a43 * B.a30  +  A.a44 * B.a40;
      a41 = A.a40 * B.a01  +  A.a41 * B.a11  +  A.a42 * B.a21  +  A.a43 * B.a31  +  A.a44 * B.a41;
      a42 = A.a40 * B.a02  +  A.a41 * B.a12  +  A.a42 * B.a22  +  A.a43 * B.a32  +  A.a44 * B.a42;
      a43 = A.a40 * B.a03  +  A.a41 * B.a13  +  A.a42 * B.a23  +  A.a43 * B.a33  +  A.a44 * B.a43;
      a44 = A.a40 * B.a04  +  A.a41 * B.a14  +  A.a42 * B.a24  +  A.a43 * B.a34  +  A.a44 * B.a44;
      
      return *this;
    }


    /**
     * Equality.
     *
     * \param  A        matrix
     * \param  eps      numerical precision
     * \return          true if matrices identical; else false
     */
    bool equals(const JMatrix5D& A,
		const double     eps = std::numeric_limits<double>::min()) const
    {
      return (fabs(a00 - A.a00) <= eps  &&
	      fabs(a01 - A.a01) <= eps  &&
	      fabs(a02 - A.a02) <= eps  &&
	      fabs(a03 - A.a03) <= eps  &&
	      fabs(a04 - A.a04) <= eps  &&
	      fabs(a10 - A.a10) <= eps  &&
	      fabs(a11 - A.a11) <= eps  &&
	      fabs(a12 - A.a12) <= eps  &&
	      fabs(a13 - A.a13) <= eps  &&
	      fabs(a14 - A.a14) <= eps  &&
	      fabs(a20 - A.a20) <= eps  &&
	      fabs(a21 - A.a21) <= eps  &&
	      fabs(a22 - A.a22) <= eps  &&
	      fabs(a23 - A.a23) <= eps  &&
	      fabs(a24 - A.a24) <= eps  &&
	      fabs(a30 - A.a30) <= eps  &&
	      fabs(a31 - A.a31) <= eps  &&
	      fabs(a32 - A.a32) <= eps  &&
	      fabs(a33 - A.a33) <= eps  &&
	      fabs(a34 - A.a34) <= eps  &&
	      fabs(a40 - A.a40) <= eps  &&
	      fabs(a41 - A.a41) <= eps  &&
	      fabs(a42 - A.a42) <= eps  &&
	      fabs(a43 - A.a43) <= eps  &&
	      fabs(a44 - A.a44) <= eps);
    }


    /**
     * Test identity.
     *
     * \param  eps      numerical precision
     * \return          true if identity matrix; else false
     */
    bool isIdentity(const double eps = std::numeric_limits<double>::min()) const
    {
      return equals(getIdentity(), eps);
    }


    /**
     * Get determinant of matrix.
     *
     * \return         determinant of matrix
     */
    double getDeterminant() const
    {
      double det = 0.0;

      det += a00 * JMatrix4D(a11, a12, a13, a14,
			     a21, a22, a23, a24,
			     a31, a32, a33, a34,
			     a41, a42, a43, a44).getDeterminant();

      det -= a01 * JMatrix4D(a10, a12, a13, a14,
			     a20, a22, a23, a24,
			     a30, a32, a33, a34,
			     a40, a42, a43, a44).getDeterminant();

      det += a02 * JMatrix4D(a10, a11, a13, a14,
			     a20, a21, a23, a24,
			     a30, a31, a33, a34,
			     a40, a41, a43, a44).getDeterminant();

      det -= a03 * JMatrix4D(a10, a11, a12, a14,
			     a20, a21, a22, a24,
			     a30, a31, a32, a34,
			     a40, a41, a42, a44).getDeterminant();

      det += a04 * JMatrix4D(a10, a11, a12, a13,
			     a20, a21, a22, a23,
			     a30, a31, a32, a33,
			     a40, a41, a42, a43).getDeterminant();

      return det;
    }


    /**
     * Transform.
     *
     * \param  __x0     x0 value
     * \param  __x1     x1 value
     * \param  __x2     x2 value
     * \param  __x3     x3 value
     * \param  __x4     x4 value
     */
    void transform(double& __x0, double& __x1, double& __x2, double& __x3, double& __x4) const
    {
      const double x0 = a00 * __x0  +  a01 * __x1  +  a02 * __x2  +  a03 * __x3  +  a04 * __x4;
      const double x1 = a10 * __x0  +  a11 * __x1  +  a12 * __x2  +  a13 * __x3  +  a14 * __x4;
      const double x2 = a20 * __x0  +  a21 * __x1  +  a22 * __x2  +  a23 * __x3  +  a24 * __x4;
      const double x3 = a30 * __x0  +  a31 * __x1  +  a32 * __x2  +  a33 * __x3  +  a34 * __x4;
      const double x4 = a40 * __x0  +  a41 * __x1  +  a42 * __x2  +  a43 * __x3  +  a44 * __x4;

      __x0 = x0;
      __x1 = x1;
      __x2 = x2;
      __x3 = x3;
      __x4 = x4;
    }


    /**
     * Read matrix from input.
     *
     * \param  in       reader
     * \param  matrix   matrix
     * \return          reader
     */
    friend inline JReader& operator>>(JReader& in, JMatrix5D& matrix)
    {
      in >> matrix.a00;  in >> matrix.a01;  in >> matrix.a02;  in >> matrix.a03;  in >> matrix.a04;
      in >> matrix.a10;  in >> matrix.a11;  in >> matrix.a12;  in >> matrix.a13;  in >> matrix.a14;
      in >> matrix.a20;  in >> matrix.a21;  in >> matrix.a22;  in >> matrix.a23;  in >> matrix.a24;
      in >> matrix.a30;  in >> matrix.a31;  in >> matrix.a32;  in >> matrix.a33;  in >> matrix.a34;
      in >> matrix.a40;  in >> matrix.a41;  in >> matrix.a42;  in >> matrix.a43;  in >> matrix.a44;

      return in;
    }


    /**
     * Write matrix to output.
     *
     * \param  out      writer
     * \param  matrix   matrix
     * \return          writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JMatrix5D& matrix)
    {
      out << matrix.a00;  out << matrix.a01;  out << matrix.a02;  out << matrix.a03;  out << matrix.a04;
      out << matrix.a10;  out << matrix.a11;  out << matrix.a12;  out << matrix.a13;  out << matrix.a14;
      out << matrix.a20;  out << matrix.a21;  out << matrix.a22;  out << matrix.a23;  out << matrix.a24;
      out << matrix.a30;  out << matrix.a31;  out << matrix.a32;  out << matrix.a33;  out << matrix.a34;
      out << matrix.a40;  out << matrix.a41;  out << matrix.a42;  out << matrix.a43;  out << matrix.a44;

      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 JMatrix5D& A)
    {
      using namespace std;

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

      out << format << A.a00 << ' ' << format << A.a01 << ' ' << format << A.a02 << ' ' << format << A.a03 << ' ' << format << A.a04 << endl;
      out << format << A.a10 << ' ' << format << A.a11 << ' ' << format << A.a12 << ' ' << format << A.a13 << ' ' << format << A.a14 << endl;
      out << format << A.a20 << ' ' << format << A.a21 << ' ' << format << A.a22 << ' ' << format << A.a23 << ' ' << format << A.a24 << endl;
      out << format << A.a30 << ' ' << format << A.a31 << ' ' << format << A.a32 << ' ' << format << A.a33 << ' ' << format << A.a34 << endl;
      out << format << A.a40 << ' ' << format << A.a41 << ' ' << format << A.a42 << ' ' << format << A.a43 << ' ' << format << A.a44 << endl;

      return out;
    }


    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double operator()(int row, int col) const
    {
      return (&a00)[row * NUMBER_OF_DIMENSIONS + col];
    }


    /**
     * Get matrix element.
     *
     * \param  row      row    number
     * \param  col      column number
     * \return          matrix element at (row,col)
     */
    double& operator()(int row, int col) 
    {
      return (&a00)[row * NUMBER_OF_DIMENSIONS + col];
    }


    static const int NUMBER_OF_DIMENSIONS = 5;

    double a00, a01, a02, a03, a04;
    double a10, a11, a12, a13, a14;
    double a20, a21, a22, a23, a24;
    double a30, a31, a32, a33, a34;
    double a40, a41, a42, a43, a44;
  };
}

#endif