/*
 * Copyright (C) 2012-2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef _GAZEBO_MATH_QUATERNION_HH_
#define _GAZEBO_MATH_QUATERNION_HH_

#include <math.h>
#include <iostream>
#include <cmath>
#include <ignition/math/Quaternion.hh>

#include "gazebo/math/Helpers.hh"
#include "gazebo/math/Angle.hh"
#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Matrix3.hh"
#include "gazebo/math/Matrix4.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace math
  {
  /// \addtogroup gazebo_math
  /// \{

  /// \class Quaternion Quaternion.hh math/gzmath.hh
  /// \brief A quaternion class
  class GZ_MATH_VISIBLE Quaternion
  {
    /// \brief Default Constructor
    public: Quaternion();

    /// \brief Constructor
    /// \param[in] _w W param
    /// \param[in] _x X param
    /// \param[in] _y Y param
    /// \param[in] _z Z param
    public: Quaternion(const double &_w, const double &_x, const double &_y,
                        const double &_z);

    /// \brief Constructor from Euler angles in radians
    /// \param[in] _roll  roll
    /// \param[in] _pitch pitch
    /// \param[in] _yaw   yaw
    public: Quaternion(const double &_roll, const double &_pitch,
                        const double &_yaw);

    /// \brief Constructor from axis angle
    /// \param[in] _axis the rotation axis
    /// \param[in] _angle the rotation angle in radians
    public: Quaternion(const Vector3 &_axis, const double &_angle);

    /// \brief Constructor
    /// \param[in] _rpy euler angles
    public: Quaternion(const Vector3 &_rpy);

    /// \brief Copy constructor
    /// \param[in] _qt Quaternion to copy
    public: Quaternion(const Quaternion &_qt);

    /// \brief Copy constructor for ignition::math::Quaterniond
    /// \param[in] _qt Ignition math quaterniond to copy
    public: Quaternion(const ignition::math::Quaterniond &_qt);

    /// \brief Destructor
    public: ~Quaternion();

    /// \brief Equal operator
    /// \param[in] _qt Quaternion to copy
    public: Quaternion &operator =(const Quaternion &_qt);

    /// \brief Convert this quaternion to an ignition::math::Quaterniond.
    /// \return This quaternion as an ignition::math::Quaterniond.
    public: ignition::math::Quaterniond Ign() const;

    /// \brief Assignment operator for ignition math
    /// \param[in] _v a new value
    /// \return The new quaternion.
    public: Quaternion &operator =(const ignition::math::Quaterniond &_v);

    /// \brief Invert the quaternion
    public: void Invert();

    /// \brief Get the inverse of this quaternion
    /// \return Inverse quaternion
    public: inline Quaternion GetInverse() const
            {
              double s = 0;
              Quaternion q(this->w, this->x, this->y, this->z);

              // use s to test if quaternion is valid
              s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;

              if (math::equal(s, 0.0))
              {
                q.w = 1.0;
                q.x = 0.0;
                q.y = 0.0;
                q.z = 0.0;
              }
              else
              {
                // deal with non-normalized quaternion
                // div by s so q * qinv = identity
                q.w =  q.w / s;
                q.x = -q.x / s;
                q.y = -q.y / s;
                q.z = -q.z / s;
              }
              return q;
            }

    /// \brief Set the quaternion to the identity
    public: void SetToIdentity();

    /// \brief Return the logarithm
    /// \return the log
    public: Quaternion GetLog() const;

    /// \brief Return the exponent
    /// \return the exp
    public: Quaternion GetExp() const;

    /// \brief Normalize the quaternion
    public: void Normalize();

    /// \brief Set the quaternion from an axis and angle
    /// \param[in] _x X axis
    /// \param[in] _y Y axis
    /// \param[in] _z Z axis
    /// \param[in] _a Angle in radians
    public: void SetFromAxis(double _x, double _y, double _z, double _a);

    /// \brief Set the quaternion from an axis and angle
    /// \param[in] _axis Axis
    /// \param[in] _a Angle in radians
    public: void SetFromAxis(const Vector3 &_axis, double _a);

    /// \brief Set this quaternion from 4 floating numbers
    /// \param[in] _u u
    /// \param[in] _x x
    /// \param[in] _y y
    /// \param[in] _z z
    public: void Set(double _u, double _x, double _y, double _z);

    /// \brief Set the quaternion from Euler angles. The order of operations
    /// is roll, pitch, yaw around a fixed body frame axis
    /// (the original frame of the object before rotation is applied).
    /// Roll is a rotation about x, pitch is about y, yaw is about z.
    /// \param[in] _vec Euler angle
    public: void SetFromEuler(const Vector3 &_vec);

    /// \brief Set the quaternion from Euler angles.
    /// \param[in] _roll Roll angle (radians).
    /// \param[in] _pitch Pitch angle (radians).
    /// \param[in] _yaw Yaw angle (radians).
    public: void SetFromEuler(double _roll, double _pitch, double _yaw);

    /// \brief Return the rotation in Euler angles
    /// \return This quaternion as an Euler vector
    public: Vector3 GetAsEuler() const;

    /// \brief Convert euler angles to a quaternion.
    /// \param[in] _vec The vector of angles to convert.
    /// \return The converted quaternion.
    public: static Quaternion EulerToQuaternion(const Vector3 &_vec);

    /// \brief Convert euler angles to quatern.
    /// \param[in] _x rotation along x
    /// \param[in] _y rotation along y
    /// \param[in] _z rotation along z
    /// \return The converted quaternion.
    public: static Quaternion EulerToQuaternion(double _x,
                                                double _y,
                                                double _z);

    /// \brief Get the Euler roll angle in radians
    /// \return the roll component
    public: double GetRoll();

    /// \brief Get the Euler pitch angle in radians
    /// \return the pitch component
    public: double GetPitch();

    /// \brief Get the Euler yaw angle in radians
    /// \return the yaw component
    public: double GetYaw();

    /// \brief Return rotation as axis and angle
    /// \param[in] _axis rotation axis
    /// \param[in] _angle ccw angle in radians
    public: void GetAsAxis(Vector3 &_axis, double &_angle) const;

    /// \brief Scale a Quaternionion
    /// \param[in] _scale Amount to scale this rotation
    public: void Scale(double _scale);

    /// \brief Addition operator
    /// \param[in] _qt quaternion for addition
    /// \return this quaternion + _qt
    public: Quaternion operator+(const Quaternion &_qt) const;

    /// \brief Addition operator
    /// \param[in] _qt quaternion for addition
    /// \return this quaternion + qt
    public: Quaternion operator+=(const Quaternion &_qt);

    /// \brief Subtraction operator
    /// \param[in] _qt quaternion to subtract
    /// \return this quaternion - _qt
    public: Quaternion operator-(const Quaternion &_qt) const;

    /// \brief Subtraction operator
    /// \param[in] _qt Quaternion for subtraction
    /// \return This quaternion - qt
    public: Quaternion operator-=(const Quaternion &_qt);

    /// \brief Multiplication operator
    /// \param[in] _q Quaternion for multiplication
    /// \return This quaternion multiplied by the parameter
    public: inline Quaternion operator*(const Quaternion &_q) const
            {
              return Quaternion(
                  this->w*_q.w - this->x*_q.x - this->y*_q.y - this->z*_q.z,
                  this->w*_q.x + this->x*_q.w + this->y*_q.z - this->z*_q.y,
                  this->w*_q.y - this->x*_q.z + this->y*_q.w + this->z*_q.x,
                  this->w*_q.z + this->x*_q.y - this->y*_q.x + this->z*_q.w);
            }

    /// \brief Multiplication operator by a scalar.
    /// \param[in] _f factor
    /// \return quaternion multiplied by the scalar
    public: Quaternion operator*(const double &_f) const;

    /// \brief Multiplication operator
    /// \param[in] _qt Quaternion for multiplication
    /// \return This quaternion multiplied by the parameter
    public: Quaternion operator*=(const Quaternion &qt);

    /// \brief Vector3 multiplication operator
    /// \param[in] _v vector to multiply
    /// \return The result of the vector multiplication
    public: Vector3 operator*(const Vector3 &_v) const;

    /// \brief Equal to operator
    /// \param[in] _qt Quaternion for comparison
    /// \return True if equal
    public: bool operator ==(const Quaternion &_qt) const;

    /// \brief Not equal to operator
    /// \param[in] _qt Quaternion for comparison
    /// \return True if not equal
    public: bool operator!=(const Quaternion &_qt) const;

    /// \brief Unary minus operator
    /// \return negates each component of the quaternion
    public: Quaternion operator-() const;

    /// \brief Rotate a vector using the quaternion
    /// \param[in] _vec vector to rotate
    /// \return the rotated vector
    public: inline Vector3 RotateVector(const Vector3 &_vec) const
            {
              Quaternion tmp(0.0, _vec.x, _vec.y, _vec.z);
              tmp = (*this) * (tmp * this->GetInverse());
              return Vector3(tmp.x, tmp.y, tmp.z);
            }

    /// \brief Do the reverse rotation of a vector by this quaternion
    /// \param[in] _vec the vector
    /// \return the reversed vector
    public: Vector3 RotateVectorReverse(Vector3 _vec) const;

    /// \brief See if a quaternion is finite (e.g., not nan)
    /// \return True if quaternion is finite
    public: bool IsFinite() const;

    /// \brief Correct any nan values in this quaternion
    public: inline void Correct()
            {
              if (!std::isfinite(this->x))
                this->x = 0;
              if (!std::isfinite(this->y))
                this->y = 0;
              if (!std::isfinite(this->z))
                this->z = 0;
              if (!std::isfinite(this->w))
                this->w = 1;

              if (math::equal(this->w, 0.0) &&
                  math::equal(this->x, 0.0) &&
                  math::equal(this->y, 0.0) &&
                  math::equal(this->z, 0.0))
              {
                this->w = 1;
              }
            }

    /// \brief Get the quaternion as a 3x3 matrix
    /// \return The 3x3 matrix form of the quaternion
    public: Matrix3 GetAsMatrix3() const;

    /// \brief Get the quaternion as a 4x4 matrix
    /// \return a 4x4 matrix
    public: Matrix4 GetAsMatrix4() const;

    /// \brief Return the X axis
    /// \return the X axis of the vector
    public: Vector3 GetXAxis() const;

    /// \brief Return the Y axis
    /// \return the Y axis of the vector
    public: Vector3 GetYAxis() const;

    /// \brief Return the Z axis
    /// \return the Z axis of the vector
    public: Vector3 GetZAxis() const;

    /// \brief Round all values to _precision decimal places
    /// \param[in] _precision the precision
    public: void Round(int _precision);

    /// \brief Dot product
    /// \param[in] _q the other quaternion
    /// \return the product
    public: double Dot(const Quaternion &_q) const;

    /// \brief Spherical quadratic interpolation
    /// given the ends and an interpolation parameter between 0 and 1
    /// \param[in] _ft the interpolation parameter
    /// \param[in] _rkP the beginning quaternion
    /// \param[in] _rkA first intermediate quaternion
    /// \param[in] _rkB second intermediate quaternion
    /// \param[in] _rkQ the end quaternion
    /// \param[in] _shortestPath when true, the rotation may be inverted to
    /// get to minimize rotation
    /// \return The result of the quadratic interpolation
    public: static Quaternion Squad(double _fT, const Quaternion &_rkP,
                const Quaternion &_rkA, const Quaternion &_rkB,
                const Quaternion &_rkQ, bool _shortestPath = false);

    /// \brief Spherical linear interpolation between 2 quaternions,
    /// given the ends and an interpolation parameter between 0 and 1
    /// \param[in] _ft the interpolation parameter
    /// \param[in] _rkP the beginning quaternion
    /// \param[in] _rkQ the end quaternion
    /// \param[in] _shortestPath when true, the rotation may be inverted to
    /// get to minimize rotation
    /// \return The result of the linear interpolation
    public: static Quaternion Slerp(double _fT, const Quaternion &_rkP,
                const Quaternion &_rkQ, bool _shortestPath = false);

    /// \brief Integrate quaternion for constant angular velocity vector
    /// along specified interval `_deltaT`.
    /// \param[in] _angularVelocity Angular velocity vector, specified in
    /// same reference frame as base of this quaternion.
    /// \param[in] _deltaT Time interval in seconds to integrate over.
    /// \return Quaternion at integrated configuration.
    public: Quaternion Integrate(const Vector3 &_angularVelocity,
                                 const double _deltaT) const;

    /// \brief w value of the quaternion
    public: double w;

    /// \brief x value of the quaternion
    public: double x;

    /// \brief y value of the quaternion
    public: double y;

    /// \brief z value of the quaternion
    public: double z;

    /// \brief Stream insertion operator
    /// \param[in] _out output stream
    /// \param[in] _q quaternion to output
    /// \return the stream
    public: friend  std::ostream &operator<<(std::ostream &_out,
                const gazebo::math::Quaternion &_q)
    {
      Vector3 v(_q.GetAsEuler());
      _out << precision(v.x, 6) << " " << precision(v.y, 6) << " "
           << precision(v.z, 6);
      return _out;
    }

    /// \brief Stream extraction operator
    /// \param[in] _in input stream
    /// \param[in] _q Quaternion to read values into
    /// \return The istream
    public: friend std::istream &operator>>(std::istream &_in,
                                             gazebo::math::Quaternion &_q)
    {
      Angle roll, pitch, yaw;

      // Skip white spaces
      _in.setf(std::ios_base::skipws);
      _in >> roll >> pitch >> yaw;

      _q.SetFromEuler(Vector3(*roll, *pitch, *yaw));

      return _in;
    }
  };
  /// \}
  }
}
#endif
