/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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 initilization 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];
			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.");
		}
	}
	/** 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(const 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(const 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 std::iterator<std::random_access_iterator_tag, value_type>
	{
	   private:
		using iterator_base =
			std::iterator<std::random_access_iterator_tag, value_type>;
		/** 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+=(iterator_base::difference_type off)
		{
			m_cur_idx += off;
			check_limits(true);
			return *this;
		}
		inline iterator operator+(iterator_base::difference_type off) const
		{
			iterator it = *this;
			it += off;
			return it;
		}
		inline iterator& operator-=(iterator_base::difference_type off)
		{
			return (*this) += (-off);
		}
		inline iterator operator-(iterator_base::difference_type off) const
		{
			iterator it = *this;
			it -= off;
			return it;
		}
		inline iterator_base::difference_type operator-(
			const iterator& it) const
		{
			return m_cur_idx - it.m_cur_idx;
		}
		inline CPose3DQuat::reference operator[](
			iterator_base::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 std::iterator<std::random_access_iterator_tag, value_type>
	{
	   private:
		using iterator_base =
			std::iterator<std::random_access_iterator_tag, value_type>;
		/** 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+=(iterator_base::difference_type off)
		{
			m_cur_idx += off;
			check_limits(true);
			return *this;
		}
		inline const_iterator operator+(
			iterator_base::difference_type off) const
		{
			const_iterator it = *this;
			it += off;
			return it;
		}
		inline const_iterator& operator-=(iterator_base::difference_type off)
		{
			return (*this) += (-off);
		}
		inline const_iterator operator-(
			iterator_base::difference_type off) const
		{
			const_iterator it = *this;
			it -= off;
			return it;
		}
		inline iterator_base::difference_type operator-(
			const const_iterator& it) const
		{
			return m_cur_idx - it.m_cur_idx;
		}
		inline CPose3DQuat::const_reference operator[](
			iterator_base::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
