#ifndef __JAXIS3D__
#define __JAXIS3D__

#include <istream>
#include <ostream>
#include <cmath>

#include "JIO/JSerialisable.hh"
#include "JGeometry3D/JVector3D.hh"
#include "JGeometry3D/JVersor3D.hh"
#include "JGeometry3D/JVersor3Z.hh"
#include "JGeometry3D/JPosition3D.hh"
#include "JGeometry3D/JDirection3D.hh"
#include "JGeometry3D/JRotation3D.hh"
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JTransformation3D.hh"
#include "JGeometry3D/JSegment3D.hh"


/**
 * \author mdejong
 */

namespace JGEOMETRY3D {}
namespace JPP { using namespace JGEOMETRY3D; }

namespace JGEOMETRY3D {

  using JIO::JReader;
  using JIO::JWriter;


  /**
   * Axis object.
   *
   * An axis object consists of a position and a direction.
   */
  class JAxis3D : 
    public JPosition3D,
    public JDirection3D
  {
  public:

    using JDirection3D::getDot;


    /**
     * Default constructor.
     */
    JAxis3D() :
      JPosition3D (),
      JDirection3D()
    {}


    /**
     * Constructor.
     *
     * \param  pos          position
     * \param  dir          direction
     */
    JAxis3D(const JVector3D& pos,
	    const JVersor3D& dir) :
      JPosition3D (pos), 
      JDirection3D(dir)
    {}


    /**
     * Constructor.
     *
     * \param  pos          position
     * \param  dir          direction
     */
    JAxis3D(const JVector3D& pos,
	    const JVersor3Z& dir) :
      JPosition3D (pos), 
      JDirection3D(dir)
    {}


    /**
     * Constructor.
     *
     * \param  segment      line segment
     */
    JAxis3D(const JSegment3D& segment) :
      JPosition3D (segment.first), 
      JDirection3D(segment.second - segment.first)
    {}

    
    /**
     * Get axis.
     *
     * \return              axis
     */
    const JAxis3D& getAxis() const 
    { 
      return *this; 
    }

    
    /**
     * Set axis.
     *
     * \param  axis         axis
     */
    void setAxis(const JAxis3D& axis)
    { 
      *this = axis; 
    }


    /**
     * Negate axis.
     *
     * \return              this axis
     */
    JAxis3D& negate()
    {
      static_cast<JPosition3D&> (*this).negate();
      static_cast<JDirection3D&>(*this).negate();

      return *this;
    }


    /**
     * Move vertex along this axis.
     *
     * \param  step         step
     */
    void move(const double step)
    {
      getPosition() += step * JPosition3D(getDirection());    
    }


    /**
     * Get longitudinal position along axis of position of closest approach with given position.
     *
     * \param  pos          position
     * \return              longitudinal position
     */
    inline double getIntersection(const JVector3D& pos) const
    {
      JPosition3D D(pos);

      D.sub(this->getPosition());

      return D.getDot(this->getDirection());
    }


    /**
     * Get longitudinal position along axis of position of closest approach with given axis.\n
     * If the axes are paralel, this position corresponds to the vertex of the given axis.
     *
     * \param  axis         axis
     * \param  precision    precision
     * \return              longitudinal position
     */
    double getIntersection(const JAxis3D& axis, const double precision = 1.0e-8) const
    {
      JPosition3D  pos(getPosition ());
      JDirection3D dir(getDirection());
      
      const JRotation3D R(axis.getDirection());
     
      // offset this position with position of given axis
 
      pos.sub(axis.getPosition());

      // rotate along given axis
      
      pos.rotate(R);
      dir.rotate(R);
      
      // longitudinal position at minimal distance of approach
      
      const double alpha = pos.getX()  * dir.getDX()  +  pos.getY()  * dir.getDY();
      const double beta  = dir.getDX() * dir.getDX()  +  dir.getDY() * dir.getDY();
      
      return (fabs(beta) > precision ? -alpha/beta : -pos.getZ());
    }


    /**
     * Get distance squared.
     *
     * \param  pos          position
     * \return              square of distance
     */
    inline double getDistanceSquared(const JVector3D& pos) const
    {
      JPosition3D D(pos);

      D.sub(this->getPosition());

      const double dz = D.getDot(this->getDirection());

      return D.getLengthSquared() - dz*dz;
    }


    /**
     * Get distance.
     *
     * \param  pos          osition
     * \return              distance
     */
    inline double getDistance(const JVector3D& pos) const
    {
      return sqrt(this->getDistanceSquared(pos));
    }


    /**
     * Rotate axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate(const JRotation3D& R)
    {
      static_cast<JPosition3D&> (*this).rotate(R);
      static_cast<JDirection3D&>(*this).rotate(R);

      return *this;
    }


    /**
     * Rotate back axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate_back(const JRotation3D& R)
    {
      static_cast<JPosition3D&> (*this).rotate_back(R);
      static_cast<JDirection3D&>(*this).rotate_back(R);

      return *this;
    }


    /**
     * Rotate around X-axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate(const JRotation3X& R)
    {
      static_cast<JPosition3D&> (*this).rotate(R);
      static_cast<JDirection3D&>(*this).rotate(R);

      return *this;
    }


    /**
     * Rotate back around X-axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate_back(const JRotation3X& R)
    {
      static_cast<JPosition3D&> (*this).rotate_back(R);
      static_cast<JDirection3D&>(*this).rotate_back(R);

      return *this;
    }


    /**
     * Rotate around Y-axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate(const JRotation3Y& R)
    {
      static_cast<JPosition3D&> (*this).rotate(R);
      static_cast<JDirection3D&>(*this).rotate(R);

      return *this;
    }


    /**
     * Rotate back around Y-axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate_back(const JRotation3Y& R)
    {
      static_cast<JPosition3D&> (*this).rotate_back(R);
      static_cast<JDirection3D&>(*this).rotate_back(R);

      return *this;
    }


    /**
     * Rotate around Z-axis.
     *
     * \param  R            rotation matrix
     * \return              this axis
     */
    JAxis3D& rotate(const JRotation3Z& R)
    {
      static_cast<JPosition3D&> (*this).rotate(R);
      static_cast<JDirection3D&>(*this).rotate(R);

      return *this;
    }


    /**
     * Rotate back around Z-axis.
     *
     * \param  R            rotation matrix
     * \return              his axis
     */
    JAxis3D& rotate_back(const JRotation3Z& R)
    {
      static_cast<JPosition3D&> (*this).rotate_back(R);
      static_cast<JDirection3D&>(*this).rotate_back(R);

      return *this;
    }


    /**
     * Rotate axis.
     *
     * \param  Q            quaternion
     * \return              this axis
     */
    JAxis3D& rotate(const JQuaternion3D& Q)
    {
      static_cast<JPosition3D&> (*this).rotate(Q);
      static_cast<JDirection3D&>(*this).rotate(Q);

      return *this;
    }


    /**
     * Transform axis to reference frame of given axis.
     *
     * \param  axis         axis
     */
    void transform(const JAxis3D& axis)
    {
      JRotation3D R  (axis.getDirection());
      JPosition3D pos(axis.getPosition ());

      transform(R, pos.rotate(R));
    }


    /**
     * Transform axis.
     *
     * The final position and direction are obtained as follows:
     *   -# rotation of the position and the direction according rotation matrix (\c R);
     *   -# offset position with position of origin in rotated system (\c pos);
     *   -# rotation of position and direction around z-axis, such that final position is in x-z plane;
     *
     * \param  R            rotation matrix
     * \param  pos          position of origin
     */
    void transform(const JRotation3D& R,
                   const JVector3D&   pos)
    {
      JPosition3D&  u = getPosition ();
      JDirection3D& v = getDirection();

      // rotate axis to system such that direction is along z-axis

      u.rotate(R);
      v.rotate(R);

      // offset with respect to origin

      u.sub(pos);

      // rotate axis to x-z plane such that position is in x-z plane

      const double phi = atan2(u.getY(), u.getX());

      const JRotation3Z r(-phi);

      u.rotate(r);
      v.rotate(r);
    }


    /**
     * Transform back axis.
     *
     * The final position and direction are obtained as follows:
     *   -# offset position with position pos;
     *   -# rotation of position and direction according matrix R;
     *
     * \param  R            rotation matrix
     * \param  pos          position of origin (before rotation)
     */
    void transform_back(const JRotation3D& R,
                        const JVector3D&   pos)
    {
      JPosition3D&  u = getPosition ();
      JDirection3D& v = getDirection();

      // offset with respect to origin

      u.add(pos);

      // rotate back axis to system with original particle direction

      u.rotate_back(R);
      v.rotate_back(R);
    }


    /**
     * Transform axis.
     *
     * \param  T            transformation
     */
    void transform(const JTransformation3D& T)
    {
      transform(T.getRotation(), T.getPosition());
    }


    /**
     * Transform back axis.
     *
     * \param  T            transformation
     */
    void transform_back(const JTransformation3D& T)
    {
      transform_back(T.getRotation(), T.getPosition());
    }


    /**
     * Read axis from input.
     *
     * \param  in           input stream
     * \param  axis         axis
     * \return              input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JAxis3D& axis)
    {
      in >> static_cast<JPosition3D&> (axis);
      in >> static_cast<JDirection3D&>(axis);

      return in;
    }


    /**
     * Write axis to output.
     *
     * \param  out          output stream
     * \param  axis         axis
     * \return              output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JAxis3D& axis)
    {
      out << static_cast<const JPosition3D&> (axis);
      out << ' ';
      out << static_cast<const JDirection3D&>(axis);

      return out;
    }


    /**
     * Read axis from input.
     *
     * \param  in           reader
     * \param  axis         axis
     * \return              reader
     */
    friend inline JReader& operator>>(JReader& in, JAxis3D& axis)
    {
      in >> static_cast<JPosition3D&> (axis);
      in >> static_cast<JDirection3D&>(axis);

      return in;
    }


    /**
     * Write axis to output.
     *
     * \param  out          writer
     * \param  axis         axis
     * \return              writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JAxis3D& axis)
    {
      out << static_cast<const JPosition3D&> (axis);
      out << static_cast<const JDirection3D&>(axis);

      return out;
    }
  };
}

#endif