/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/Stringifyable.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CQuaternion.h>
#include <mrpt/math/TPose3DQuat.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/poses/CPose.h>
#include <mrpt/poses/poses_frwds.h>

namespace mrpt::poses
{
/** A class used to store a 3D pose as a translation (x,y,z) and a quaternion
 * (qr,qx,qy,qz).
 *
 *  For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint,
 * or refer
 *    to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry
 * tutorial</a> in the wiki.
 *
 *  To access the translation use x(), y() and z(). To access the rotation, use
 * CPose3DQuat::quat().
 *
 *  This class also behaves like a STL container, since it has begin(), end(),
 * iterators, and can be accessed with the [] operator
 *   with indices running from 0 to 6 to access the  [x y z qr qx qy qz] as if
 * they were a vector. Thus, a CPose3DQuat can be used
 *   as a 7-vector anywhere the MRPT math functions expect any kind of vector.
 *
 *  This class and CPose3D are very similar, and they can be converted to the
 * each other automatically via transformation constructors.
 *
 * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion),
 * mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic
 * version of this class,  mrpt::math::CQuaternion, CPoseOrPoint
 * \ingroup poses_grp
 */
class CPose3DQuat :
    public CPose<CPose3DQuat, 7>,
    public mrpt::serialization::CSerializable,
    public mrpt::Stringifyable
{
  DEFINE_SERIALIZABLE(CPose3DQuat, mrpt::poses)
  DEFINE_SCHEMA_SERIALIZABLE()
 public:
  /** The translation vector [x,y,z] */
  mrpt::math::CVectorFixedDouble<3> m_coords;

  /** The quaternion [0]:w, [1-3]:x,y,z */
  mrpt::math::CQuaternionDouble m_quat;

 public:
  /** Read/Write access to the quaternion representing the 3D rotation. */
  inline mrpt::math::CQuaternionDouble& quat() { return m_quat; }
  /** Read-only access to the quaternion representing the 3D rotation. */
  inline const mrpt::math::CQuaternionDouble& quat() const { return m_quat; }
  /** Read/Write access to the translation vector in R^3. */
  inline mrpt::math::CVectorFixedDouble<3>& xyz() { return m_coords; }
  /** Read-only access to the translation vector in R^3. */
  inline const mrpt::math::CVectorFixedDouble<3>& xyz() const { return m_coords; }
  /** Default constructor, initialize translation to zeros and quaternion to
   * no rotation. */
  inline CPose3DQuat() : m_quat() { m_coords[0] = m_coords[1] = m_coords[2] = 0.; }

  /** Constructor which left all the quaternion members un-initialized, for
   * use when speed is critical; Use UNINITIALIZED_POSE as argument to this
   * constructor. */
  inline CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions) :
      m_quat(mrpt::math::UNINITIALIZED_QUATERNION)
  {
  }
  /** \overload */
  inline CPose3DQuat(TConstructorFlags_Poses) : m_quat(mrpt::math::UNINITIALIZED_QUATERNION) {}

  /** Constructor with Initialization of the pose - the quaternion is
   * normalized to make sure it's unitary */
  inline CPose3DQuat(
      const double x, const double y, const double z, const mrpt::math::CQuaternionDouble& q) :
      m_quat(q)
  {
    m_coords[0] = x;
    m_coords[1] = y;
    m_coords[2] = z;
    m_quat.normalize();
  }

  /** Constructor from a CPose3D */
  explicit CPose3DQuat(const CPose3D& p);

  /** Constructor from lightweight object. */
  CPose3DQuat(const mrpt::math::TPose3DQuat& p) : m_quat(p.qr, p.qx, p.qy, p.qz)
  {
    x() = p.x;
    y() = p.y;
    z() = p.z;
  }

  mrpt::math::TPose3DQuat asTPose() const;

  /** Constructor from a 4x4 homogeneous transformation matrix.
   */
  explicit CPose3DQuat(const mrpt::math::CMatrixDouble44& M);

  /** Returns the corresponding 4x4 homogeneous transformation matrix for the
   * point(translation) or pose (translation+orientation).
   * \sa getInverseHomogeneousMatrix
   */
  void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const;
  /** Returns a 7x1 vector with [x y z qr qx qy qz]' */
  void asVector(vector_t& v) const;

  /**  Makes \f$ this = A \oplus B \f$  this method is slightly more efficient
   * than "this= A + B;" since it avoids the temporary object.
   *  \note A or B can be "this" without problems.
   * \sa inverseComposeFrom, composePoint
   */
  void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B);

  /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
   * than "this= A - B;" since it avoids the temporary object.
   *  \note A or B can be "this" without problems.
   * \sa composeFrom, composePoint
   */
  void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B);

  /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
   * \sa composeFrom, inverseComposePoint
   */
  void composePoint(
      const double lx,
      const double ly,
      const double lz,
      double& gx,
      double& gy,
      double& gz,
      mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
      mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr) const;

  /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
   * \sa composePoint, composeFrom
   */
  void inverseComposePoint(
      const double gx,
      const double gy,
      const double gz,
      double& lx,
      double& ly,
      double& lz,
      mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint = nullptr,
      mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose = nullptr) const;

  /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
   *  POINT1 and POINT1 can be anything supporing [0],[1],[2].
   * \sa composePoint    */
  template <class POINT1, class POINT2>
  inline void composePoint(const POINT1& L, POINT2& G) const
  {
    composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
  }

  /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa
   * inverseComposePoint */
  template <class POINT1, class POINT2>
  inline void inverseComposePoint(const POINT1& G, POINT2& L) const
  {
    inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
  }

  /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.  \sa
   * composePoint    */
  inline CPoint3D operator+(const CPoint3D& L) const
  {
    CPoint3D G;
    composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
    return G;
  }

  /**  Computes the 3D point G such as \f$ G = this \oplus L \f$.  \sa
   * composePoint    */
  inline mrpt::math::TPoint3D operator+(const mrpt::math::TPoint3D& L) const
  {
    mrpt::math::TPoint3D G;
    composePoint(L[0], L[1], L[2], G[0], G[1], G[2]);
    return G;
  }

  /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by
   * the scalar). */
  virtual void operator*=(const double s);

  /** Make \f$ this = this \oplus b \f$  */
  inline CPose3DQuat& operator+=(const CPose3DQuat& b)
  {
    composeFrom(*this, b);
    return *this;
  }

  /** Return the composed pose \f$ ret = this \oplus p \f$  */
  inline CPose3DQuat operator+(const CPose3DQuat& p) const
  {
    CPose3DQuat ret;
    ret.composeFrom(*this, p);
    return ret;
  }

  /** Make \f$ this = this \ominus b \f$  */
  inline CPose3DQuat& operator-=(const CPose3DQuat& b)
  {
    inverseComposeFrom(*this, b);
    return *this;
  }

  /** Return the composed pose \f$ ret = this \ominus p \f$  */
  inline CPose3DQuat operator-(const CPose3DQuat& p) const
  {
    CPose3DQuat ret;
    ret.inverseComposeFrom(*this, p);
    return ret;
  }

  /** Convert this pose into its inverse, saving the result in itself. \sa
   * operator- */
  void inverse();

  /** Returns a human-readable textual representation of the object as:
   *  `"[x y z qw qx qy qz]"`
   * \sa fromString
   */
  std::string asString() const override
  {
    return mrpt::format(
        "[%f %f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2], m_quat[0], m_quat[1],
        m_quat[2], m_quat[3]);
  }

  /** Set the current object value from a string generated by 'asString' (eg:
   * "[0.02 1.04 -0.8 1 0 0 0]" )
   * \sa asString
   * \exception std::exception On invalid format
   */
  void fromString(const std::string& s);

  /** Same as fromString, but without requiring the square brackets in the
   * string */
  void fromStringRaw(const std::string& s);

  static CPose3DQuat FromString(const std::string& s)
  {
    CPose3DQuat o;
    o.fromString(s);
    return o;
  }

  /** Read only [] operator */
  inline double operator[](unsigned int i) const
  {
    switch (i)
    {
      case 0:
        return m_coords[0];
      case 1:
        return m_coords[1];
      case 2:
        return m_coords[2];
      case 3:
        return m_quat[0];  // w
      case 4:
        return m_quat[1];  // x
      case 5:
        return m_quat[2];  // y
      case 6:
        return m_quat[3];  // z
      default:
        throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
    }
  }
  /** Read/write [] operator */
  inline double& operator[](unsigned int i)
  {
    switch (i)
    {
      case 0:
        return m_coords[0];
      case 1:
        return m_coords[1];
      case 2:
        return m_coords[2];
      case 3:
        return m_quat[0];
      case 4:
        return m_quat[1];
      case 5:
        return m_quat[2];
      case 6:
        return m_quat[3];
      default:
        throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
    }
  }

  /** Computes the spherical coordinates of a 3D point as seen from the 6D
   * pose specified by this object.
   *  For the coordinate system see the top of this page.
   *  If the matrix pointers are not nullptr, the Jacobians will be also
   * computed for the range-yaw-pitch variables wrt the passed 3D point and
   * this 7D pose.
   */
  void sphericalCoordinates(
      const mrpt::math::TPoint3D& point,
      double& out_range,
      double& out_yaw,
      double& out_pitch,
      mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint = nullptr,
      mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose = nullptr) const;

 public:
  /** Used to emulate CPosePDF types, for example, in
   * mrpt::graphs::CNetworkOfPoses */
  using type_value = CPose3DQuat;
  enum
  {
    is_3D_val = 1
  };
  static constexpr bool is_3D() { return is_3D_val != 0; }
  enum
  {
    rotation_dimensions = 3
  };
  enum
  {
    is_PDF_val = 1
  };
  static constexpr bool is_PDF() { return is_PDF_val != 0; }
  inline const type_value& getPoseMean() const { return *this; }
  inline type_value& getPoseMean() { return *this; }
  /** @name STL-like methods and typedefs
   @{   */
  /** The type of the elements */
  using value_type = double;
  using reference = double&;
  using const_reference = double;
  using size_type = std::size_t;
  using difference_type = std::ptrdiff_t;

  // size is constant
  enum
  {
    static_size = 7
  };
  static constexpr size_type size() { return static_size; }
  static constexpr bool empty() { return false; }
  static constexpr size_type max_size() { return static_size; }
  static inline void resize(size_t n)
  {
    if (n != static_size)
      throw std::logic_error(
          format("Try to change the size of CPose3DQuat to %u.", static_cast<unsigned>(n)));
  }

  inline void assign(size_t N, const double val)
  {
    if (N != 7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
    m_coords.fill(val);
    m_quat.fill(val);
  }

  struct iterator
  {
   public:
    // iterator traits
    using difference_type = std::ptrdiff_t;
    using value_type = double;
    using pointer = const double*;
    using reference = const double&;
    using iterator_category = std::random_access_iterator_tag;

   private:
    /** A reference to the source of this iterator */
    CPose3DQuat* m_obj{nullptr};
    /** The iterator points to this element. */
    size_t m_cur_idx{0};
    /** The type of the matrix elements */
    using T = value_type;

    inline void check_limits([[maybe_unused]] bool allow_end = false) const
    {
#ifdef _DEBUG
      ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
      if (m_cur_idx > (allow_end ? 7u : 6u)) THROW_EXCEPTION("Index out of range in iterator.");
#endif
    }

   public:
    inline bool operator<(const iterator& it2) const { return m_cur_idx < it2.m_cur_idx; }
    inline bool operator>(const iterator& it2) const { return m_cur_idx > it2.m_cur_idx; }
    inline iterator() = default;
    inline iterator(CPose3DQuat& obj, size_t start_idx) : m_obj(&obj), m_cur_idx(start_idx)
    {
      check_limits(true); /*Dont report as error an iterator to end()*/
    }
    inline CPose3DQuat::reference operator*() const
    {
      check_limits();
      return (*m_obj)[m_cur_idx];
    }
    inline iterator& operator++()
    {
      check_limits();
      ++m_cur_idx;
      return *this;
    }
    inline iterator operator++(int)
    {
      iterator it = *this;
      ++*this;
      return it;
    }
    inline iterator& operator--()
    {
      --m_cur_idx;
      check_limits();
      return *this;
    }
    inline iterator operator--(int)
    {
      iterator it = *this;
      --*this;
      return it;
    }
    inline iterator& operator+=(difference_type off)
    {
      m_cur_idx += off;
      check_limits(true);
      return *this;
    }
    inline iterator operator+(difference_type off) const
    {
      iterator it = *this;
      it += off;
      return it;
    }
    inline iterator& operator-=(difference_type off) { return (*this) += (-off); }
    inline iterator operator-(difference_type off) const
    {
      iterator it = *this;
      it -= off;
      return it;
    }
    inline difference_type operator-(const iterator& it) const { return m_cur_idx - it.m_cur_idx; }
    inline CPose3DQuat::reference operator[](difference_type off) const
    {
      return (*m_obj)[m_cur_idx + off];
    }
    inline bool operator==(const iterator& it) const
    {
      return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
    }
    inline bool operator!=(const iterator& it) const { return !(operator==(it)); }
  };  // end iterator

  struct const_iterator
  {
   public:
    // iterator traits
    using difference_type = std::ptrdiff_t;
    using value_type = double;
    using pointer = const double*;
    using reference = const double&;
    using iterator_category = std::random_access_iterator_tag;

   private:
    /** A reference to the source of this iterator */
    const CPose3DQuat* m_obj{nullptr};
    /** The iterator points to this element. */
    size_t m_cur_idx{0};
    /** The type of the matrix elements */
    using T = value_type;

    inline void check_limits([[maybe_unused]] bool allow_end = false) const
    {
#ifdef _DEBUG
      ASSERTMSG_(m_obj != nullptr, "non initialized iterator");
      if (m_cur_idx > (allow_end ? 7u : 6u)) THROW_EXCEPTION("Index out of range in iterator.");
#endif
    }

   public:
    inline bool operator<(const const_iterator& it2) const { return m_cur_idx < it2.m_cur_idx; }
    inline bool operator>(const const_iterator& it2) const { return m_cur_idx > it2.m_cur_idx; }
    inline const_iterator() = default;
    inline const_iterator(const CPose3DQuat& obj, size_t start_idx) :
        m_obj(&obj), m_cur_idx(start_idx)
    {
      check_limits(true); /*Dont report as error an iterator to end()*/
    }
    inline CPose3DQuat::const_reference operator*() const
    {
      check_limits();
      return (*m_obj)[m_cur_idx];
    }
    inline const_iterator& operator++()
    {
      check_limits();
      ++m_cur_idx;
      return *this;
    }
    inline const_iterator operator++(int)
    {
      const_iterator it = *this;
      ++*this;
      return it;
    }
    inline const_iterator& operator--()
    {
      --m_cur_idx;
      check_limits();
      return *this;
    }
    inline const_iterator operator--(int)
    {
      const_iterator it = *this;
      --*this;
      return it;
    }
    inline const_iterator& operator+=(difference_type off)
    {
      m_cur_idx += off;
      check_limits(true);
      return *this;
    }
    inline const_iterator operator+(difference_type off) const
    {
      const_iterator it = *this;
      it += off;
      return it;
    }
    inline const_iterator& operator-=(difference_type off) { return (*this) += (-off); }
    inline const_iterator operator-(difference_type off) const
    {
      const_iterator it = *this;
      it -= off;
      return it;
    }
    inline difference_type operator-(const const_iterator& it) const
    {
      return m_cur_idx - it.m_cur_idx;
    }
    inline CPose3DQuat::const_reference operator[](difference_type off) const
    {
      return (*m_obj)[m_cur_idx + off];
    }
    inline bool operator==(const const_iterator& it) const
    {
      return m_obj == it.m_obj && m_cur_idx == it.m_cur_idx;
    }
    inline bool operator!=(const const_iterator& it) const { return !(operator==(it)); }
  };  // end const_iterator

  using reverse_iterator = std::reverse_iterator<iterator>;
  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
  inline iterator begin() { return iterator(*this, 0); }
  inline iterator end() { return iterator(*this, static_size); }
  inline const_iterator begin() const { return const_iterator(*this, 0); }
  inline const_iterator end() const { return const_iterator(*this, static_size); }
  inline reverse_iterator rbegin() { return reverse_iterator(end()); }
  inline const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
  inline reverse_iterator rend() { return reverse_iterator(begin()); }
  inline const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }

  void swap(CPose3DQuat& o)
  {
    std::swap(o.m_coords, m_coords);
    o.m_quat.swap(m_quat);
  }

  /** @} */

  void setToNaN() override;

};  // End of class def.

std::ostream& operator<<(std::ostream& o, const CPose3DQuat& p);

/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
 * than a pose with all its arguments multiplied by "-1") */
CPose3DQuat operator-(const CPose3DQuat& p);
/**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa
 * inverseComposePoint    */
CPoint3D operator-(const CPoint3D& G, const CPose3DQuat& p);
/**  Computes the 3D point L such as \f$ L = G \ominus this \f$.  \sa
 * inverseComposePoint    */
mrpt::math::TPoint3D operator-(const mrpt::math::TPoint3D& G, const CPose3DQuat& p);

bool operator==(const CPose3DQuat& p1, const CPose3DQuat& p2);
bool operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2);

}  // namespace mrpt::poses
