#ifndef __JROTATION3D__
#define __JROTATION3D__

#include <cmath>

#include "JGeometry3D/JMatrix3D.hh"
#include "JGeometry3D/JAngle3D.hh"
#include "JGeometry2D/JRotation2D.hh"


namespace JGEOMETRY3D {

  namespace {
    using JGEOMETRY2D::JRotation2D;
  }


  /**
   * Rotation around X-axis.
   */
  class JRotation3X :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3X() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * \param  phi        rotation angle (clock wise) [rad]
     */
    JRotation3X(const double phi) :
      JRotation2D(phi)
    {}
  };


  /**
   * Rotation around Y-axis.
   */
  class JRotation3Y :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3Y() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * \param  phi        rotation angle (clock wise) [rad]
     */
    JRotation3Y(const double phi) :
      JRotation2D(phi)
    {}
  };


  /**
   * Rotation around Z-axis.
   */
  class JRotation3Z :
    public JRotation2D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3Z() :
      JRotation2D()
    {}


    /**
     * Constructor.
     *
     * \param  phi        rotation angle (clock wise) [rad]
     */
    JRotation3Z(const double phi) :
      JRotation2D(phi)
    {}
  };


  /**
   * Rotation matrix
   */
  class JRotation3D :
    public JMatrix3D
  {
  public:
    /**
     * Default constructor (= identity matrix).
     */
    JRotation3D() :
      JMatrix3D()
    {}


    /**
     * Constructor.
     * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis
     * and the back rotation of a vector parallel to the z-axis ends up in the given direction.
     * 
     * \param  dir        direction
     */
    JRotation3D(const JAngle3D& dir) :
      JMatrix3D()
    {
      const double ct = cos(dir.getTheta());
      const double st = sin(dir.getTheta());
      const double cp = cos(dir.getPhi());
      const double sp = sin(dir.getPhi());

      a00_ =  ct*cp;  a01_ =  ct*sp;  a02_ = -st;
      a10_ = -sp;     a11_ =  cp;     a12_ =  0;
      a20_ =  st*cp;  a21_ =  st*sp;  a22_ = +ct;
    }


    /**
     * Constructor.
     * 
     * \param  M          2D rotation matrix around X-axis
     */
    JRotation3D(const JRotation3X& M) :
      JMatrix3D()
    {
      a11_ =  M.a00();  a12_ =  M.a01();
      a21_ =  M.a10();  a22_ =  M.a11();
    }


    /**
     * Constructor.
     * 
     * \param  M          2D rotation matrix around Y-axis
     */
    JRotation3D(const JRotation3Y& M) :
      JMatrix3D()
    {
      a00_ =  M.a00();  a02_ =  M.a01();
      a20_ =  M.a10();  a22_ =  M.a11();
    }


    /**
     * Constructor.
     * 
     * \param  M          2D rotation matrix around Z-axis
     */
    JRotation3D(const JRotation3Z& M) :
      JMatrix3D()
    {
      a00_ =  M.a00();  a01_ =  M.a01();
      a10_ =  M.a10();  a11_ =  M.a11();
    }


    /**
     * Transpose.
     */
    JRotation3D& transpose()
    {
      JMatrix3D::transpose();

      return *this;
    }


    /**
     * Matrix multiplication.
     *
     * \param  B          JMatrix3D
     * \return            this matrix
     */
    JRotation3D& multiply(const JRotation3D& B)
    {
      JMatrix3D::multiply(B);

      return *this;
    }


    /**
     * Rotate.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void rotate(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;
    }

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

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

#endif