/*
 * 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_POSE_HH_
#define _GAZEBO_POSE_HH_

#include <iostream>

#include <ignition/math/Pose3.hh>

#include "gazebo/math/Vector3.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/util/system.hh"

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

    /// \class Pose Pose.hh math/gzmath.hh
    /// \brief Encapsulates a position and rotation in three space
    class GZ_MATH_VISIBLE Pose
    {
      /// \brief math::Pose(0, 0, 0, 0, 0, 0)
      public: static const Pose Zero;

      /// \brief Default constructors
      public: Pose();

      /// \brief Constructor
      /// \param[in] _pos A position
      /// \param[in] _rot A rotation
      public: Pose(const Vector3 &_pos, const Quaternion &_rot);

      /// \brief Constructor
      /// \param[in] _x x position in meters.
      /// \param[in] _y y position in meters.
      /// \param[in] _z z position in meters.
      /// \param[in] _roll Roll (rotation about X-axis) in radians.
      /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
      /// \param[in] _yaw Yaw (rotation about z-axis) in radians.
      public: Pose(double _x, double _y, double _z,
                   double _roll, double _pitch, double _yaw);

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

      /// \brief Copy constructor for ignition math
      /// \param[in] _pose Pose to copy
      public: Pose(const ignition::math::Pose3d &_pose);

      /// \brief Destructor
      public: virtual ~Pose();

      /// \brief Set the pose from a Vector3 and a Quaternion
      /// \param[in] _pos The position.
      /// \param[in] _rot The rotation.
      public: void Set(const Vector3 &_pos, const Quaternion &_rot);

      /// \brief Set the pose from  pos and rpy vectors
      /// \param[in] _pos The position.
      /// \param[in] _rpy The rotation expressed as Euler angles.
      public: void Set(const Vector3 &_pos, const Vector3 &_rpy);

      /// \brief Set the pose from a six tuple.
      /// \param[in] _x x position in meters.
      /// \param[in] _y y position in meters.
      /// \param[in] _z z position in meters.
      /// \param[in] _roll Roll (rotation about X-axis) in radians.
      /// \param[in] _pitch Pitch (rotation about y-axis) in radians.
      /// \param[in] _yaw Pitch (rotation about z-axis) in radians.
      public: void Set(double _x, double _y, double _z,
                       double _roll, double _pitch, double _yaw);

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

      /// \brief Fix any nan values
      public: inline void Correct()
              {
                this->pos.Correct();
                this->rot.Correct();
              }

      /// \brief Get the inverse of this pose
      /// \return the inverse pose
      public: Pose GetInverse() const;

      /// \brief Addition operator
      /// A is the transform from O to P specified in frame O
      /// B is the transform from P to Q specified in frame P
      /// then, B + A is the transform from O to Q specified in frame O
      /// \param[in] _pose Pose to add to this pose
      /// \return The resulting pose
      public: Pose operator+(const Pose &_pose) const;

      /// \brief Add-Equals operator
      /// \param[in] _pose Pose to add to this pose
      /// \return The resulting pose
      public: const Pose &operator+=(const Pose &_pose);

      /// \brief Negation operator
      /// A is the transform from O to P in frame O
      /// then -A is transform from P to O specified in frame P
      /// \return The resulting pose
      public: inline Pose operator-() const
              {
                return Pose() - *this;
              }

      /// \brief Subtraction operator
      /// A is the transform from O to P in frame O
      /// B is the transform from O to Q in frame O
      /// B - A is the transform from P to Q in frame P
      /// \param[in] _pose Pose to subtract from this one
      /// \return The resulting pose
      public: inline Pose operator-(const Pose &_pose) const
              {
                return Pose(this->CoordPositionSub(_pose),
                            this->CoordRotationSub(_pose.rot));
              }

      /// \brief Subtraction operator
      /// \param[in] _pose Pose to subtract from this one
      /// \return The resulting pose
      public: const Pose &operator-=(const Pose &_pose);

      /// \brief Equality operator
      /// \param[in] _pose Pose for comparison
      /// \return True if equal
      public: bool operator ==(const Pose &_pose) const;

      /// \brief Inequality operator
      /// \param[in] _pose Pose for comparison
      /// \return True if not equal
      public: bool operator!=(const Pose &_pose) const;

      /// \brief Multiplication operator
      /// \param[in] _pose the other pose
      /// \return itself
      public: Pose operator*(const Pose &_pose);

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

      /// \brief Equal operator for ignition math
      /// \param[in] _pose Pose to copy
      public: Pose &operator=(const ignition::math::Pose3d &_pose);

      /// \brief Add one point to a vector: result = this + pos
      /// \param[in] _pos Position to add to this pose
      /// \return the resulting position
      public: Vector3 CoordPositionAdd(const Vector3 &_pos) const;

      /// \brief Add one point to another: result = this + pose
      /// \param[in] _pose The Pose to add
      /// \return The resulting position
      public: Vector3 CoordPositionAdd(const Pose &_pose) const;

      /// \brief Subtract one position from another: result = this - pose
      /// \param[in] _pose Pose to subtract
      /// \return The resulting position
      public: inline Vector3 CoordPositionSub(const Pose &_pose) const
              {
                Quaternion tmp(0.0,
                    this->pos.x - _pose.pos.x,
                    this->pos.y - _pose.pos.y,
                    this->pos.z - _pose.pos.z);

                tmp = _pose.rot.GetInverse() * (tmp * _pose.rot);
                return Vector3(tmp.x, tmp.y, tmp.z);
              }

      /// \brief Add one rotation to another: result =  this->rot + rot
      /// \param[in] _rot Rotation to add
      /// \return The resulting rotation
      public: Quaternion CoordRotationAdd(const Quaternion &_rot) const;

      /// \brief Subtract one rotation from another: result = this->rot - rot
      /// \param[in] _rot The rotation to subtract
      /// \return The resulting rotation
      public: inline Quaternion CoordRotationSub(const Quaternion &_rot) const
              {
                Quaternion result(_rot.GetInverse() * this->rot);
                result.Normalize();
                return result;
              }

      /// \brief Find the inverse of a pose; i.e., if b = this + a, given b and
      ///        this, find a
      /// \param[in] _b the other pose
      public: Pose CoordPoseSolve(const Pose &_b) const;

      /// \brief Reset the pose
      public: void Reset();

      /// \brief Rotate vector part of a pose about the origin
      /// \param[in] _rot rotation
      /// \return the rotated pose
      public: Pose RotatePositionAboutOrigin(const Quaternion &_rot) const;

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

      /// \brief Convert this pose to an ignition::math::Pose3d object.
      /// \return This pose represented as an ignition::math::Pose3d.
      public: ignition::math::Pose3d Ign() const;

      /// \brief Stream insertion operator
      /// \param[in] _out output stream
      /// \param[in] _pose pose to output
      /// \return the stream
      public: friend std::ostream &operator<<(std::ostream &_out,
                                              const gazebo::math::Pose &_pose)
              {
                _out << _pose.pos << " " << _pose.rot;
                return _out;
              }

      /// \brief Stream extraction operator
      /// \param[in] _in the input stream
      /// \param[in] _pose the pose
      /// \return the stream
      public: friend std::istream &operator>>(std::istream &_in,
                gazebo::math::Pose &_pose)
            {
              // Skip white spaces
              _in.setf(std::ios_base::skipws);
              _in >> _pose.pos >> _pose.rot;
              return _in;
            }

      /// \brief The position
      public: Vector3 pos;

      /// \brief The rotation
      public: Quaternion rot;
    };
    /// \}
  }
}
#endif
