#ifndef __JMATRIX3D__
#define __JMATRIX3D__

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

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


/**
 * \author mdejong
 */

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

namespace JMATH {

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


  /**
   * 3 x 3 matrix
   */
  class JMatrix3D :
    public JMath  <JMatrix3D>,
    public JEquals<JMatrix3D>
  {
  public:

    using JMath<JMatrix3D>::mul;


    /**
     * Default constructor.
     */
    JMatrix3D() :
      a00(0.0), a01(0.0), a02(0.0),
      a10(0.0), a11(0.0), a12(0.0),
      a20(0.0), a21(0.0), a22(0.0)
    {}


    /**
     * Contructor.
     * 
     * \param   __a00   (0,0)
     * \param   __a01   (0,1)
     * \param   __a02   (0,2)
     * \param   __a10   (1,0)
     * \param   __a11   (1,1)
     * \param   __a12   (1,2)
     * \param   __a20   (2,0)
     * \param   __a21   (2,1)
     * \param   __a22   (2,2)
     */
    JMatrix3D(const double __a00, const double __a01, const double __a02,
	      const double __a10, const double __a11, const double __a12,
	      const double __a20, const double __a21, const double __a22) :
      a00(__a00), a01(__a01), a02(__a02),
      a10(__a10), a11(__a11), a12(__a12),
      a20(__a20), a21(__a21), a22(__a22)
    {}


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

      return matrix;
    }


    /**
     * Set to identity matrix
     *
     * \return          this matrix
     */
    JMatrix3D& setIdentity()
    {
      a00 = 1.0;  a01 = 0.0;  a02 = 0.0;
      a10 = 0.0;  a11 = 1.0;  a12 = 0.0;
      a20 = 0.0;  a21 = 0.0;  a22 = 1.0;

      return *this;
    }


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

      return matrix;
    }


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


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

      return *this;
    }


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

      swap(a10, a01);
      swap(a20, a02);
      swap(a21, a12);

      return *this;
    }


    /**
     * Negate matrix.
     *
     * \return          -this matrix
     */
    JMatrix3D& negate()
    {
      a00 = -a00;  a01 = -a01;  a02 = -a02;
      a10 = -a10;  a11 = -a11;  a12 = -a12;
      a20 = -a20;  a21 = -a21;  a22 = -a22;

      return *this;
    }


    /**
     * Matrix addition.
     *
     * \param  A        matrix
     * \return          this matrix + A
     */
    JMatrix3D& add(const JMatrix3D& A)
    {
      a00 += A.a00;  a01 += A.a01;  a02 += A.a02;
      a10 += A.a10;  a11 += A.a11;  a12 += A.a12;
      a20 += A.a20;  a21 += A.a21;  a22 += A.a22;
  
      return *this;
    }


    /**
     * Matrix subtraction.
     *
     * \param  A        matrix
     * \return          this matrix - A
     */
    JMatrix3D& sub(const JMatrix3D& A)
    {
      a00 -= A.a00;  a01 -= A.a01;  a02 -= A.a02;
      a10 -= A.a10;  a11 -= A.a11;  a12 -= A.a12;
      a20 -= A.a20;  a21 -= A.a21;  a22 -= A.a22;
  
      return *this;
    }


    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix * factor
     */
    JMatrix3D& mul(const double factor)
    {
      a00 *= factor; a01 *= factor; a02 *= factor;
      a10 *= factor; a11 *= factor; a12 *= factor;
      a20 *= factor; a21 *= factor; a22 *= factor;

      return *this;
    }


    /**
     * Scale matrix.
     *
     * \param  factor   factor
     * \return          this matrix / factor
     */
    JMatrix3D& div(const double factor)
    {
      a00 /= factor; a01 /= factor; a02 /= factor;
      a10 /= factor; a11 /= factor; a12 /= factor;
      a20 /= factor; a21 /= factor; a22 /= factor;

      return *this;
    }


    /**
     * Matrix multiplication.
     *
     * \param  A        matrix
     * \param  B        matrix
     * \return          this matrix
     */
    JMatrix3D& mul(const JMatrix3D& A, 
		   const JMatrix3D& B)
    {
      a00 = A.a00 * B.a00  +  A.a01 * B.a10  +  A.a02 * B.a20;
      a01 = A.a00 * B.a01  +  A.a01 * B.a11  +  A.a02 * B.a21;
      a02 = A.a00 * B.a02  +  A.a01 * B.a12  +  A.a02 * B.a22;
      
      a10 = A.a10 * B.a00  +  A.a11 * B.a10  +  A.a12 * B.a20;
      a11 = A.a10 * B.a01  +  A.a11 * B.a11  +  A.a12 * B.a21;
      a12 = A.a10 * B.a02  +  A.a11 * B.a12  +  A.a12 * B.a22;
      
      a20 = A.a20 * B.a00  +  A.a21 * B.a10  +  A.a22 * B.a20;
      a21 = A.a20 * B.a01  +  A.a21 * B.a11  +  A.a22 * B.a21;
      a22 = A.a20 * B.a02  +  A.a21 * B.a12  +  A.a22 * B.a22;
      
      return *this;
    }


    /**
     * Equality.
     *
     * \param  A        matrix
     * \param  eps      numerical precision
     * \return          true if matrices identical; else false
     */
    bool equals(const JMatrix3D& 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(a10 - A.a10) <= eps  &&
	      fabs(a11 - A.a11) <= eps  &&
	      fabs(a12 - A.a12) <= eps  &&
	      fabs(a20 - A.a20) <= eps  &&
	      fabs(a21 - A.a21) <= eps  &&
	      fabs(a22 - A.a22) <= 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 * (a11 * a22 - a21 * a12);
      det -= a01 * (a10 * a22 - a20 * a12);
      det += a02 * (a10 * a21 - a20 * a11);

      return det;
    }


    /**
     * Transform.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void transform(double& __x, double& __y, double& __z) const
    {
      const double x = a00 * __x  +  a01 * __y  +  a02 * __z;
      const double y = a10 * __x  +  a11 * __y  +  a12 * __z;
      const double z = a20 * __x  +  a21 * __y  +  a22 * __z;

      __x = x;
      __y = y;
      __z = z;
    }


    /**
     * Read matrix from input.
     *
     * \param  in       reader
     * \param  matrix   matrix
     * \return          reader
     */
    friend inline JReader& operator>>(JReader& in, JMatrix3D& matrix)
    {
      in >> matrix.a00;  in >> matrix.a01;  in >> matrix.a02;
      in >> matrix.a10;  in >> matrix.a11;  in >> matrix.a12;
      in >> matrix.a20;  in >> matrix.a21;  in >> matrix.a22;

      return in;
    }


    /**
     * Write matrix to output.
     *
     * \param  out      writer
     * \param  matrix   matrix
     * \return          writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JMatrix3D& matrix)
    {
      out << matrix.a00;  out << matrix.a01;  out << matrix.a02;
      out << matrix.a10;  out << matrix.a11;  out << matrix.a12;
      out << matrix.a20;  out << matrix.a21;  out << matrix.a22;

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

      const JFormat format(out, getFormat<JMatrix3D>(JFormat_t(10, 3, std::ios::fixed | std::ios::showpos)));
      
      out << format << A.a00 << ' ' << format << A.a01 << ' ' << format << A.a02 << endl;
      out << format << A.a10 << ' ' << format << A.a11 << ' ' << format << A.a12 << endl;
      out << format << A.a20 << ' ' << format << A.a21 << ' ' << format << A.a22 << endl;

      return out;
    }


    double a00, a01, a02;
    double a10, a11, a12;
    double a20, a21, a22;
  };
}

#endif