/*---------------------------------------------------------------
	FILE: CPoses2DSequence.h
	USE: See below.

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CPoses2DSequence_H
#define CPoses2DSequence_H

#include <MRPT/MRML/CPose2D.h>
#include <MRPT/UTILS/CSerializable.h>

namespace MRML
{

	/** This class stores a sequence of relative, incremental 2D poses. It is useful as the bases storing unit for more complex probability particles and for computing the absolute pose of any intermediate pose.
	 *
	 * \sa CPose2D, CMultiMetricMap
	 */
	class CPoses2DSequence : public CSerializable
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPoses2DSequence )
	public:
		/** Default constructor
		 */
		CPoses2DSequence();

		/** Copy constructor
		 */
		CPoses2DSequence(const CPoses2DSequence &obj);

		/** Returns the poses count in the sequence:
		 */
		int	 posesCount();

		/** Reads the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
		 * \exception std::exception On invalid index value
		 */
		void  getPose(unsigned int ind, CPose2D &outPose);

		/** Changes the stored pose at index "ind", where the first one is 0, the last "posesCount() - 1"
		 * \exception std::exception On invalid index value
		 */
		void  changePose(unsigned int ind, CPose2D &inPose);

		/** Appends a new pose at the end of sequence. Remember that poses are relative, incremental to the last one.
		 */
		void  appendPose(CPose2D &newPose);

		/** Clears the sequence.
		 */
		void  clear();

		/** Returns the absolute pose of a robot after moving "n" poses, so for "n=0" the origin pose (0,0,0deg) is returned, for "n=1" the first pose is returned, and for "n=posesCount()", the pose
		 *  of robot after moving ALL poses is returned, all of them relative to the starting pose.
		 * \exception std::exception On invalid index value
		 * \sa absolutePoseAfterAll
		 */
		CPose2D  absolutePoseOf(unsigned int n);

		/** A shortcut for "absolutePoseOf( posesCount() )".
		 * \sa absolutePoseOf, posesCount
		 */
		CPose2D  absolutePoseAfterAll();

		/** Returns the traveled distance after moving "n" poses, so for "n=0" it returns 0, for "n=1" the first traveled distance, and for "n=posesCount()", the total
		 *  distance after ALL movements.
		 * \exception std::exception On invalid index value
		 * \sa computeTraveledDistanceAfterAll
		 */
		float  computeTraveledDistanceAfter(unsigned int n);

		/** Returns the traveled distance after ALL movements.
		 *   A shortcut for "computeTraveledDistanceAfter( posesCount() )".
		 * \sa computeTraveledDistanceAfter
		 */
		float  computeTraveledDistanceAfterAll();

	private:
		/** The internal sequence of poses, stored as relative, incremental poses, thus each one is situated just at the end point of last one, where the first one is referenced to (0,0,0deg)
		 */
		std::vector<CPose2D>	poses;

	}; // End of class def.

} // End of namespace

#endif
