/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CSensFrameProbSequence_H
#define CSensFrameProbSequence_H

#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/MRML/CSensoryFrame.h>
#include <MRPT/MRML/CPosePDF.h>
#include <MRPT/MRML/CPose3DPDF.h>

namespace MRML
{
	/** This class stores a sequence of <Probabilistic Pose,SensorialFrame> pairs, thus a "metric map" can be totally determined with this information.
	 *     The pose of the sensory frame is not deterministic, but described by some PDF. Full 6D poses are used.
	 *
	 * \sa CSensoryFrame, CPosePDF
	 */
	class CSensFrameProbSequence : public UTILS::CSerializable
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CSensFrameProbSequence )

	public:
		/** Constructor
		  */
		CSensFrameProbSequence();

		/** Copy constructor
		  */
		CSensFrameProbSequence( const CSensFrameProbSequence &o );

		/** Copy constructor
		  */
		CSensFrameProbSequence & operator = ( const CSensFrameProbSequence& o);

		/**  Destructor:
		  */
		virtual ~CSensFrameProbSequence();

		/** Returns the pairs count.
		  */
		size_t size() const;

		/** Access to the i'th pair, first one is index '0'. NOTE: This method
		  *  returns pointers to the objects inside the list, nor a copy of them,
		  *  so <b>do neither modify them nor delete them</b>.
		  * NOTE: You can pass a NULL pointer if you dont need one of the two variables to be returned.
		  * \exception std::exception On index out of bounds.
		  */
		void  get(size_t index, CPose3DPDF *&out_posePDF, CSensoryFrame *&out_SF );

		/** Access to the i'th pair, first one is index '0'. NOTE: This method
		  *  returns pointers to the objects inside the list, nor a copy of them,
		  *  so <b>do neither modify them nor delete them</b>.
		  * NOTE: You can pass a NULL pointer if you dont need one of the two variables to be returned.
		  * \exception std::exception On index out of bounds.
		  */
		void  get(size_t index, const CPose3DPDF *&out_posePDF, const CSensoryFrame *&out_SF ) const;

		/** Changes the i'th pair, first one is index '0'.
		  *  The referenced object is COPIED, so you can freely destroy the object passed as parameter after calling this.
		  *  If one of the pointers is NULL, the corresponding contents of the current i'th pair is not modified (i.e. if you want just to modify one of the values).
		  * \exception std::exception On index out of bounds.
		  * \sa insert, get, remove
		  */
		void  set(size_t index, CPose3DPDF *in_posePDF, CSensoryFrame *in_SF );

		/** Changes the i'th pair, first one is index '0'.
		  *  The referenced object is COPIED, so you can freely destroy the object passed as parameter after calling this.
		  *  If one of the pointers is NULL, the corresponding contents of the current i'th pair is not modified (i.e. if you want just to modify one of the values).
		  * This version for 2D PDFs just converts the 2D PDF into 3D before calling the 3D version.
		  * \exception std::exception On index out of bounds.
		  * \sa insert, get, remove
		  */
		void  set(size_t index, CPosePDF *in_posePDF, CSensoryFrame *in_SF );

		/** Deletes the i'th pair, first one is index '0'.
		  * \exception std::exception On index out of bounds.
		  * \sa insert, get, set
		  */
		void  remove(size_t index);

		/** Add a new pair to the sequence. The objects are copied, so original ones can be free if desired
		  *  after insertion.
		  */
		void  insert( const CPose3DPDF *in_posePDF, const CSensoryFrame *in_SF );

		/** Add a new pair to the sequence. The objects are copied, so original ones can be free if desired
		  *  after insertion.
		  * This version for 2D PDFs just converts the 2D PDF into 3D before calling the 3D version.
		  */
		void  insert( const CPosePDF *in_posePDF, const CSensoryFrame *in_SF );

		/** Remove all stored pairs.
		  * \sa remove
		  */
		void  clear();

		/** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system. */
		void changeCoordinatesOrigin( const CPose3D  &newOrigin );

	private:

		typedef std::pair<CPose3DPDF*,CSensoryFrame*> TPosePDFSensFramePair;

		typedef std::deque<TPosePDFSensFramePair> TPosePDFSensFramePairList;

		/** The stored data */
		TPosePDFSensFramePairList		m_posesObsPairs;

	}; // End of class def.

	// This must be added to any CSerializable derived class:
	DEFINE_SERIALIZABLE_POST( CSensFrameProbSequence )


} // End of namespace

#endif
