/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#ifndef CPOINT_H
#define CPOINT_H

#include <mrpt/poses/CPoseOrPoint.h>
#include <mrpt/math/CMatrixTemplateNumeric.h>

namespace mrpt
{
	namespace poses
	{
		/** A base class for representing a point in 2D or 3D.
		 *   For more information refer to the <a href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
		 * \note This class is based on the CRTP design pattern
		 * \sa CPoseOrPoint, CPose
		 * \ingroup poses_grp
		 */
		template <class DERIVEDCLASS>
		class CPoint : public CPoseOrPoint<DERIVEDCLASS>
		{
		public:
			/** @name Methods common to all 2D or 3D points
			    @{ */

			/** Scalar addition of all coordinates.
			  * This is diferent from poses/point composition, which is implemented as "+" operators in classes derived from "CPose"
			  */
			template <class OTHERCLASS>
			inline void AddComponents(const OTHERCLASS &b)
			{
				const int dims = std::min( size_t(DERIVEDCLASS::static_size), size_t(OTHERCLASS::is3DPoseOrPoint() ? 3:2));
				for (int i=0;i<dims;i++)
					static_cast<DERIVEDCLASS*>(this)->m_coords[i]+= static_cast<const OTHERCLASS*>(&b)->m_coords[i];
			}

			/** Scalar multiplication. */
			inline void operator *=(const double s)
			{
				for (int i=0;i<DERIVEDCLASS::static_size;i++)
					static_cast<DERIVEDCLASS*>(this)->m_coords[i] *= s;
			}

			/** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
			inline void getAsVector(mrpt::math::CVectorDouble &v) const
			{
				v.resize(DERIVEDCLASS::static_size);
				for (int i=0;i<DERIVEDCLASS::static_size;i++)
					v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
			}
			//! \overload
			inline mrpt::math::CVectorDouble getAsVector() const { mrpt::math::CVectorDouble v; getAsVector(v); return v; }

			/** 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
			{
				out_HM.unit(4,1.0);
				out_HM.get_unsafe(0,3)= static_cast<const DERIVEDCLASS*>(this)->x();
				out_HM.get_unsafe(1,3)= static_cast<const DERIVEDCLASS*>(this)->y();
				if (DERIVEDCLASS::is3DPoseOrPoint())
					out_HM.get_unsafe(2,3)= static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
			}

			/** Returns a human-readable textual representation of the object (eg: "[0.02 1.04]" )
			* \sa fromString
			*/
			void asString(std::string &s) const
			{
				s = (!DERIVEDCLASS::is3DPoseOrPoint()) ?
					mrpt::format("[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y()) :
					mrpt::format("[%f %f %f]",static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y(), static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
			}
			inline std::string asString() const { std::string s; asString(s); return s; }

			/** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" )
			* \sa asString
			* \exception std::exception On invalid format
			*/
			void fromString(const std::string &s)
			{
				mrpt::math::CMatrixDouble  m;
				if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
				ASSERT_EQUAL_(mrpt::math::size(m,1),1)
				ASSERT_EQUAL_(mrpt::math::size(m,2),DERIVEDCLASS::static_size)
				for (int i=0;i<DERIVEDCLASS::static_size;i++)
					static_cast<DERIVEDCLASS*>(this)->m_coords[i] = m.get_unsafe(0,i);
			}

			inline const double &operator[](unsigned int i) const { return static_cast<const DERIVEDCLASS*>(this)->m_coords[i]; }
			inline       double &operator[](unsigned int i)       { return static_cast<DERIVEDCLASS*>(this)->m_coords[i]; }

			/** @} */

		}; // End of class def.

	/** Dumps a point as a string [x,y] or [x,y,z]  */
	template <class DERIVEDCLASS>
	std::ostream &operator << (std::ostream& o, const CPoint<DERIVEDCLASS>& p)
	{
		o << "(" << p[0] << "," << p[1];
		if (p.is3DPoseOrPoint())	o << "," << p[2];
		o <<")";
		return o;
	}

	/** Used by STL algorithms */
	template <class DERIVEDCLASS>
	bool operator < (const CPoint<DERIVEDCLASS> &a, const CPoint<DERIVEDCLASS> &b)
	{
		if (a.x()<b.x()) return true;
		else
		{
			if (!a.is3DPoseOrPoint())
				return a.y()<b.y();
			else  if (a.y()<b.y())
				return true;
			else return a[2]<b[2];
		}
	}

	template <class DERIVEDCLASS>
	bool operator==(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
	{
		for (int i=0;i<DERIVEDCLASS::static_size;i++)
			if (p1[i]!=p2[i])	return false; //-V550
		return true;
	}

	template <class DERIVEDCLASS>
	bool operator!=(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
	{
		for (int i=0;i<DERIVEDCLASS::static_size;i++)
			if (p1[i]!=p2[i])	return true; //-V550
		return false;
	}



	} // End of namespace
} // End of namespace

#endif
