#ifndef __JQUATERNION3D__
#define __JQUATERNION3D__

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

#include "JIO/JSerialisable.hh"
#include "JLang/JEquals.hh"
#include "JMath/JMath.hh"
#include "JMath/JConstants.hh"
#include "JGeometry3D/JVector3D.hh"
#include "JGeometry3D/JVersor3D.hh"


/**
 * \author mdejong
 */

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

namespace JGEOMETRY3D {

  using JIO::JReader;
  using JIO::JWriter;
  using JMATH::JMath;
  using JLANG::JEquals;
  
  
  /**
   * This class represents a rotation.
   */
  struct JQuaternion2D {
  protected:
    /**
     * Default constructor.
     * This constructor corresponds to the identity operation.
     */
    JQuaternion2D() :
      __a(1.0),
      __u(0.0)
    {}


    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion2D(const double theta) :
      __a(cos(0.5*theta)),
      __u(sin(0.5*theta))
    {}


  public:
    /**
     * Get a value.
     *
     * \return               a value
     */
    double getA() const 
    { 
      return __a;
    }


    /**
     * Raise quaternion to given power.
     *
     * \param  y              power
     * \return                this object
     */
    inline JQuaternion2D& pow(const double y)
    {
      const double theta = atan2(__u, __a) * y;

      __a = cos(theta);
      __u = sin(theta);

      return *this;
    }


    /**
     * Write quaternion from input.
     *
     * \param  in            input stream
     * \param  quaternion    quaternion
     * \return               input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JQuaternion2D& quaternion)
    {
      return in >> quaternion.__a >> quaternion.__u;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           output stream
     * \param  quaternion    quaternion
     * \return               output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion2D& quaternion)
    {
      const JFormat format(out, getFormat<JQuaternion3D>(JFormat_t(9, 6, std::ios::fixed | std::ios::showpos)));

      return out << format << quaternion.__a << ' '
		 << format << quaternion.__u;
    }
    

    /**
     * Read quaternion from input.
     *
     * \param  in            reader
     * \param  quaternion    quaternion
     * \return               reader
     */
    friend inline JReader& operator>>(JReader& in, JQuaternion2D& quaternion)
    {
      return in >> quaternion.__a >> quaternion.__u;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           writer
     * \param  quaternion    quaternion
     * \return               writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JQuaternion2D& quaternion)
    {
      return out << quaternion.__a << quaternion.__u;
    }


  protected:
    double __a;
    double __u;
  };

  
  /**
   * This class represents a rotation around the x-axis.
   */
  struct JQuaternion3X :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3X() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3X(const double theta) :
      JQuaternion2D(theta)
    {}


    /**
     * Get b value.
     *
     * \return               b value
     */
    double getB() const 
    { 
      return __u;
    }


    /**
     * Raise quaternion to given power.
     *
     * \param  y              power
     * \return                this object
     */
    inline JQuaternion3X& pow(const double y)
    {
      JQuaternion2D::pow(y);

      return *this;
    }
  };

  
  /**
   * This class represents a rotation around the y-axis.
   */
  struct JQuaternion3Y  :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3Y() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3Y(const double theta) :
      JQuaternion2D(theta)
    {}

    
    /**
     * Get c value.
     *
     * \return               c value
     */
    double getC() const 
    { 
      return __u;
    }


    /**
     * Raise quaternion to given power.
     *
     * \param  y              power
     * \return                this object
     */
    inline JQuaternion3Y& pow(const double y)
    {
      JQuaternion2D::pow(y);

      return *this;
    }
  };

  
  /**
   * This class represents a rotation around the z-axis.
   */
  struct JQuaternion3Z  :
    public JQuaternion2D
  {
    /**
     * Default constructor.
     */
    JQuaternion3Z() :
      JQuaternion2D()
    {}

    
    /**
     * Constructor.
     *
     * \param  theta         rotation angle [rad]
     */
    JQuaternion3Z(const double theta) :
      JQuaternion2D(theta)
    {}

    
    /**
     * Get d value.
     *
     * \return               d value
     */
    double getD() const 
    { 
      return __u;
    }


    /**
     * Raise quaternion to given power.
     *
     * \param  y              power
     * \return                this object
     */
    inline JQuaternion3Z& pow(const double y)
    {
      JQuaternion2D::pow(y);

      return *this;
    }
  };
  

  /**
   * Data structure for unit quaternion in three dimensions.
   *
   * This class implements the JMATH::JMath and JLANG::JEquals interfaces.
   */
  class JQuaternion3D :
    public JMath  <JQuaternion3D>,
    public JMath  <JQuaternion3D, JQuaternion3X>,
    public JMath  <JQuaternion3D, JQuaternion3Y>,
    public JMath  <JQuaternion3D, JQuaternion3Z>,
    public JEquals<JQuaternion3D>
  {
  public:

    using JMath<JQuaternion3D>::mul;
    using JMath<JQuaternion3D, JQuaternion3X>::mul;
    using JMath<JQuaternion3D, JQuaternion3Y>::mul;
    using JMath<JQuaternion3D, JQuaternion3Z>::mul;


    struct decomposition;  // forward declaration

    
    /**
     * Default constructor.
     */
    JQuaternion3D() :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {}


    /**
     * Constructor.
     *
     * \param  a             a value
     * \param  b             b value
     * \param  c             c value
     * \param  d             d value
     */
    JQuaternion3D(const double a,
		  const double b,
		  const double c,
		  const double d) :
      __a(a),
      __b(b),
      __c(c),
      __d(d)
    {
      normalise();
    }


    /**
     * Constructor.
     *
     * This constructor represents a rotation around the given axis by the given angle.
     *
     * \param  theta         rotation angle [rad]
     * \param  dir           rotation axis
     */
    JQuaternion3D(const double     theta,
		  const JVersor3D& dir)
    {
      const double ct = cos(0.5*theta);
      const double st = sin(0.5*theta);

      __a = ct;
      __b = st * dir.getDX();
      __c = st * dir.getDY();
      __d = st * dir.getDZ();
    }


    /**
     * Constructor.
     *
     * \param  w             weight
     * \param  dir           rotation axis
     */
    JQuaternion3D(const double     w,
		  const JVector3D& dir)
    {
      __a = w;
      __b = dir.getX();
      __c = dir.getY();
      __d = dir.getZ();

      normalise();
    }


    /**
     * Constructor.
     *
     * \param  qx            rotation around x-axis
     */
    JQuaternion3D(const JQuaternion3X& qx) :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {
      mul(qx);
    }


    /**
     * Constructor.
     *
     * \param  qy            rotation around y-axis
     */
    JQuaternion3D(const JQuaternion3Y& qy) :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {
      mul(qy);
    }


    /**
     * Constructor.
     *
     * \param  qz            rotation around x-axis
     */
    JQuaternion3D(const JQuaternion3Z& qz) :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {
      mul(qz);
    }


    /**
     * Constructor.
     *
     * \param  qx            rotation around x-axis
     * \param  qy            rotation around y-axis
     * \param  qz            rotation around z-axis
     */
    JQuaternion3D(const JQuaternion3X& qx,
		  const JQuaternion3Y& qy,
		  const JQuaternion3Z& qz) :
      __a(1.0),
      __b(0.0),
      __c(0.0),
      __d(0.0)
    {
      mul(qz).mul(qy).mul(qx);

      normalise();
    }


    /**
     * Get identity quaternion
     *
     * \return               identity quaternion
     */
    static const JQuaternion3D& getIdentity()
    {
      static const JQuaternion3D Q = JQuaternion3D().setIdentity();

      return Q;
    }


    /**
     * Get quaternion.
     *
     * \return               quaternion
     */
    const JQuaternion3D& getQuaternion() const
    {
      return static_cast<const JQuaternion3D&>(*this);
    }


    /**
     * Get quaternion.
     *
     * \return               quaternion
     */
    JQuaternion3D& getQuaternion()
    {
      return static_cast<JQuaternion3D&>(*this);
    }


    /**
     * Set quaternion.
     *
     * \param  quaternion    quaternion
     */
    void setQuaternion(const JQuaternion3D& quaternion)
    {
      static_cast<JQuaternion3D&>(*this) = quaternion;
    }

    
    /**
     * Type conversion operator.
     *
     * \return               position
     */
    operator JVector3D() const
    {
      return JVector3D(this->getB(), this->getC(), this->getD());
    }

    
    /**
     * Type conversion operator.
     *
     * \return               direction
     */
    operator JVersor3D() const
    {
      return JVersor3D(this->getB(), this->getC(), this->getD());
    }


    /**
     * Get rotation angle.
     *
     * \return               angle [rad]
     */
    inline double getAngle() const
    {
      return atan2(sqrt(this->getB()*this->getB() +
			this->getC()*this->getC() +
			this->getD()*this->getD()),
		   this->getA()) * 2.0;
    }


    /**
     * Get a value.
     *
     * \return               a value
     */
    double getA() const 
    { 
      return __a;
    }

    
    /**
     * Get b value.
     *
     * \return               b value
     */
    double getB() const 
    { 
      return __b;
    }


    /**
     * Get c value.
     *
     * \return               c value
     */
    double getC() const 
    { 
      return __c;
    }


    /**
     * Get d value.
     *
     * \return               d value
     */
    double getD() const 
    { 
      return __d;
    }


    /**
     * Set to identity quaternion
     *
     * \return               this quaternion
     */
    JQuaternion3D& setIdentity()
    {
      __a = 1.0;
      __b = 0.0;
      __c = 0.0;
      __d = 0.0;

      return *this;
    }


    /** 
     * Conjugate quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& conjugate()
    {
      __b = -__b;
      __c = -__c;
      __d = -__d;

      return *this;
    }


    /** 
     * Negate quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& negate()
    {
      __a = -__a;
      __b = -__b;
      __c = -__c;
      __d = -__d;

      return *this;
    }


    /** 
     * Add quaternion.
     *
     * \param  quaternion    quaternion
     * \return               this quaternion
     */
    JQuaternion3D& add(const JQuaternion3D& quaternion)
    {
      __a += quaternion.getA();
      __b += quaternion.getB();
      __c += quaternion.getC();
      __d += quaternion.getD();

      return *this;
    }


    /** 
     * Subtract quaternion.
     *
     * \param  quaternion    quaternion
     * \return               this quaternion
     */
    JQuaternion3D& sub(const JQuaternion3D& quaternion)
    {
      __a -= quaternion.getA();
      __b -= quaternion.getB();
      __c -= quaternion.getC();
      __d -= quaternion.getD();

      return *this;
    }


    /** 
     * Scale quaternion.
     *
     * \param  factor        multiplication factor
     * \return               this quaternion
     */
    JQuaternion3D& mul(const double factor)
    {
      __a *= factor;
      __b *= factor;
      __c *= factor;
      __d *= factor;

      return *this;
    }


    /** 
     * Scale quaternion.
     *
     * \param  factor        division factor
     * \return               this quaternion
     */
    JQuaternion3D& div(const double factor)
    {
      __a /= factor;
      __b /= factor;
      __c /= factor;
      __d /= factor;

      return *this;
    }


    /**
     * Quaternion multiplication.
     *
     * This method evaluates the Hamilton product (also called cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first, const JQuaternion3X& second)
    {
      __a =  first.getA() * second.getA() - first.getB() * second.getB();
      __b =  first.getA() * second.getB() + first.getB() * second.getA();
      __c =  first.getC() * second.getA() + first.getD() * second.getB();
      __d = -first.getC() * second.getB() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplication.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
		       const JQuaternion3Y& second)
    {
      __a =  first.getA() * second.getA() - first.getC() * second.getC();
      __b =  first.getB() * second.getA() - first.getD() * second.getC();
      __c =  first.getA() * second.getC() + first.getC() * second.getA();
      __d =  first.getB() * second.getC() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplication.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
		       const JQuaternion3Z& second)
    {
      __a =  first.getA() * second.getA() - first.getD() * second.getD();
      __b =  first.getB() * second.getA() + first.getC() * second.getD();
      __c = -first.getB() * second.getD() + first.getC() * second.getA();
      __d =  first.getA() * second.getD() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplication.
     *
     * This method evaluates the Hamilton product (or cross product).
     *
     * \param  first         first  quaternion
     * \param  second        second quaternion
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3D& first,
		       const JQuaternion3D& second)
    {
      __a = first.getA() * second.getA() - first.getB() * second.getB() - first.getC() * second.getC() - first.getD() * second.getD();
      __b = first.getA() * second.getB() + first.getB() * second.getA() + first.getC() * second.getD() - first.getD() * second.getC();
      __c = first.getA() * second.getC() - first.getB() * second.getD() + first.getC() * second.getA() + first.getD() * second.getB();
      __d = first.getA() * second.getD() + first.getB() * second.getC() - first.getC() * second.getB() + first.getD() * second.getA();
      
      return *this;
    }


    /**
     * Quaternion multiplication.
     *
     * \param  qx            rotation around x-axis
     * \param  qy            rotation around y-axis
     * \param  qz            rotation around x-axis
     * \return               this quaternion
     */
    JQuaternion3D& mul(const JQuaternion3X& qx,
		       const JQuaternion3Y& qy,
		       const JQuaternion3Z& qz)
    {
      return *this = JQuaternion3D().setIdentity().mul(qz).mul(qy).mul(qx);
    }


    /**
     * Rotate.
     *
     * \param  __x      x value
     * \param  __y      y value
     * \param  __z      z value
     */
    void rotate(double& __x, double& __y, double& __z) const
    {
      const double qx = 2.0 * (__c*__z - __d*__y);
      const double qy = 2.0 * (__d*__x - __b*__z);
      const double qz = 2.0 * (__b*__y - __c*__x);

      __x = __x + __c*qz - __d*qy + __a*qx;
      __y = __y - __b*qz + __a*qy + __d*qx;
      __z = __z + __a*qz + __b*qy - __c*qx;
    }


    /**
     * 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 qx = 2.0 * (__d*__y - __c*__z);
      const double qy = 2.0 * (__b*__z - __d*__x);
      const double qz = 2.0 * (__c*__x - __b*__y);

      __x = __x - __c*qz + __d*qy + __a*qx;
      __y = __y + __b*qz + __a*qy - __d*qx;
      __z = __z + __a*qz - __b*qy + __c*qx;
    }
    

    /** 
     * Check equality.
     *
     * \param  quaternion    quaternion
     * \param  precision     numerical precision
     * \return               true if quaternions are equal; else false
     */
    bool equals(const JQuaternion3D& quaternion,
      		const double         precision = std::numeric_limits<double>::min()) const
    {
      return (fabs(getA() - quaternion.getA()) <= precision && 
	      fabs(getB() - quaternion.getB()) <= precision &&
	      fabs(getC() - quaternion.getC()) <= precision &&
	      fabs(getD() - quaternion.getD()) <= precision);
    }


    /**
     * Test identity.
     *
     * \param  precision     precision
     * \return               true if identity quaternion; else false
     */
    bool isIdentity(const double precision = std::numeric_limits<double>::min()) const
    {
      if (fabs(getA()) <= precision) {

	if (fabs(getB()) <= precision)
	  return ((fabs(getC())       <= precision && fabs(fabs(getD()) - 1.0) <= precision)  ||
		  (fabs(fabs(getC()) - 1.0) <= precision && fabs(getD())       <= precision));
	else
	  return (fabs(fabs(getB()) - 1.0) <= precision && 
		  fabs(getC())             <= precision &&
		  fabs(getD())             <= precision);

      } else {

	return (fabs(fabs(getA()) - 1.0) <= precision && 
		fabs(getB())             <= precision &&
		fabs(getC())             <= precision &&
		fabs(getD())             <= precision);
      }
    }


    /**
     * Get length squared.
     *
     * \return               square of length
     */
    double getLengthSquared() const 
    { 
      return getA()*getA() + getB()*getB() + getC()*getC() + getD()*getD();
    }


    /**
     * Get length.
     *
     * \return               length
     */
    double getLength() const 
    { 
      return sqrt(getLengthSquared());
    }


    /**
     * Get squared of distance to quaternion.
     *
     * \param  quaternion    quaternion
     * \return               square of distance
     */
    double getDistanceSquared(const JQuaternion3D& quaternion) const
    {
      return JQuaternion3D(quaternion).sub(*this).getLengthSquared();
    }


    /**
     * Get distance to quaternion.
     *
     * \param  quaternion    quaternion
     * \return               distance
     */
    double getDistance(const JQuaternion3D& quaternion) const
    {
      return sqrt(getDistanceSquared(quaternion));
    }


    /**
     * Get dot product.
     *
     * \param  quaternion    quaternion
     * \return               dot product
     */
    double getDot(const JQuaternion3D& quaternion) const
    {
      return
	getA() * quaternion.getA() -
	getB() * quaternion.getB() -
	getC() * quaternion.getC() -
	getD() * quaternion.getD();
    }


    /**
     * Get conjugate of this quaternion.
     *
     * \return               conjugate quaternion
     */
    JQuaternion3D getConjugate() const
    {
      return JQuaternion3D(*this).conjugate();
    }


    /**
     * Normalise quaternion.
     *
     * \return               this quaternion
     */
    JQuaternion3D& normalise()
    {
      const double v = getLength();

      if (v != 0.0) {
	mul(1.0 / v);
      }

      return *this;
    }


    /**
     * Raise quaternion to given power.
     *
     * \param  y              power
     * \return                this object
     */
    inline JQuaternion3D& pow(const double y)
    {
      const double v = sqrt(getB() * getB()  +
			    getC() * getC()  +
			    getD() * getD());

      if (v != 0.0) {

	const JVersor3D u(getB(), getC(), getD());

	const double theta = atan2(v, getA());

	*this = JQuaternion3D(2.0 * theta * y, u);
      }

      return *this;
    }


    /**
     * Interpolation between quaternions.
     * The result is equal to <tt>*this = (1 - alpha) * (*this) + (alpha) * (object)</tt>.
     *
     * \param  object         object
     * \param  alpha          interpolation factor <tt>[0, 1]</tt>
     * \return                this object
     */
    inline JQuaternion3D& interpolate(const JQuaternion3D& object,
				      const double         alpha)
    {
      const double MAXIMAL_DOT_PRODUCT = 0.9995;
      
      JQuaternion3D v0(*this);
      JQuaternion3D v1(object);

      v0.normalise();
      v1.normalise();

      double dot = v0.getDot(v1);

      if (dot < 0.0) {
	v1  = -v1;
	dot = -dot;
      }

      double s1 = alpha;
      double s0 = 1.0 - alpha;

      if (dot <= MAXIMAL_DOT_PRODUCT) {

	const double theta_0 = acos(dot);
	const double theta_1 = theta_0 * alpha;
      
	s1 = sin(theta_1) / sin(theta_0);
	s0 = cos(theta_1) - dot * s1;
      }

      *this = (s0 * v0)  +  (s1 * v1);

      return normalise();
    }


    /**
     * Write quaternion from input.
     *
     * \param  in            input stream
     * \param  quaternion    quaternion
     * \return               input stream
     */
    friend inline std::istream& operator>>(std::istream& in, JQuaternion3D& quaternion)
    {
      in >> quaternion.__a;
      in >> quaternion.__b;
      in >> quaternion.__c;
      in >> quaternion.__d;

      quaternion.normalise();
      
      return in;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           output stream
     * \param  quaternion    quaternion
     * \return               output stream
     */
    friend inline std::ostream& operator<<(std::ostream& out, const JQuaternion3D& quaternion)
    {
      const JFormat format(out, getFormat<JQuaternion3D>(JFormat_t(9, 6, std::ios::fixed | std::ios::showpos)));

      out << format << quaternion.getA() << ' '
	  << format << quaternion.getB() << ' '
	  << format << quaternion.getC() << ' '
	  << format << quaternion.getD();

      return out;
    }
    

    /**
     * Read quaternion from input.
     *
     * \param  in            reader
     * \param  quaternion    quaternion
     * \return               reader
     */
    friend inline JReader& operator>>(JReader& in, JQuaternion3D& quaternion)
    {
      in >> quaternion.__a;
      in >> quaternion.__b;
      in >> quaternion.__c;
      in >> quaternion.__d;

      return in;
    }


    /**
     * Write quaternion to output.
     *
     * \param  out           writer
     * \param  quaternion    quaternion
     * \return               writer
     */
    friend inline JWriter& operator<<(JWriter& out, const JQuaternion3D& quaternion)
    {
      out << quaternion.__a;
      out << quaternion.__b;
      out << quaternion.__c;
      out << quaternion.__d;

      return out;
    }


  protected:
    double __a;
    double __b;
    double __c;
    double __d;
  };


  /**
   * Auxiliary data structure for decomposition of quaternion in twist and swing quaternions.
   */
  struct JQuaternion3D::decomposition {
    /**
     * Constructor.
     *
     * \param  Q             quaternion
     * \param  dir           direction
     */
    decomposition(const JQuaternion3D& Q,
		  const JVector3D&     dir)
    {
      twist = JQuaternion3D(Q.getA(), dir * dir.getDot(Q));           // rotation around given axis
      swing = JQuaternion3D(Q * twist.getConjugate());                //
    }


    /**
     * Get quaternion.
     *
     * \return               quaternion
     */
    JQuaternion3D getQuaternion() const
    {
      return swing * twist;
    }

    JQuaternion3D twist;    //!< rotation around parallel axis
    JQuaternion3D swing;    //!< rotation around perpendicular axis
  };



  /**
   * Get space angle between quanternions.
   *
   * \param  first          first  quanternion
   * \param  second         second quanternion
   * \return                angle [deg]
   */
  inline double getAngle(const JQuaternion3D& first,
                         const JQuaternion3D& second)
  {
    double dot = (first.getA() * second.getA()  +
		  first.getB() * second.getB()  +
		  first.getC() * second.getC()  +
		  first.getD() * second.getD());
    
    dot = 2.0 * dot * dot  -  1.0;

    if      (dot > +1.0)
      return   0.0;
    else if (dot < -1.0)
      return 180.0;
    else
      return 0.5 * acos(dot) * 180.0 / JMATH::PI;
  }
}

#endif