/* +---------------------------------------------------------------------------+
   |          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 CSENSORIALFRAME_H
#define CSENSORIALFRAME_H

#include <MRPT/MRML/CObservation.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/MRML/CPointsMap.h>
#include <MRPT/MRML/CObservation2DRangeScan.h>


namespace MRML
{
	class	CSimplePointsMap;

	/** Declares a class for storing a "sensory frame", thus
	 *      a set of "observations" of the robot, taken exactly
	 *	   from the same location.The "observations" can be of
	 *	   many different kinds.<br>
	  *  New observations can be added using:<br>
	  * \code
		  CObservation		*o = new CObservationXXX(...);
		  CSensoryFrame	 sf;
		  sf.insert(o);
		\endcode

		The following methods are equivalent for adding new observations to a "sensory frame":
			* CSensoryFrame::operator +=
			* CSensoryFrame::push_back
			* CSensoryFrame::insert

		To examine the objects within a sensory frame, the following methods exist:
			* CSensoryFrame::getObservationByClass : Looks for some specific observation class.
			* CSensoryFrame::begin : To iterate over all observations.
			* CSensoryFrame::getObservationByIndex : To query by index.

		Notice that observations objects are automatically deleted on
		Sensorial Frame destruction or clear.
	 * \sa CObservation
	 */
	class CSensoryFrame : public UTILS::CSerializable
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CSensoryFrame )
		// -------------------------------------------------------------------------------
		//   HACK: For compatibility with old datasets (See CSensoryFrame.cpp)
		// -------------------------------------------------------------------------------
		static CLASSINIT _init_CSensorialFrame;
		static  TRuntimeClassId classCSensorialFrame;

	private:
		/** Auxiliary points map, built only under request through buildAuxPointsMap */
		CObservation2DRangeScan::CAuxMapWrapper	m_auxMap;

	public:
		 /** Default constructor
		  */
		 CSensoryFrame();

		 /** Copy constructor
		  */
		 CSensoryFrame( const CSensoryFrame &);

		 /** Build (only on the first call), and return a simple points map built from the observations in the SF (options can be optionally provided to the map building process)
		   */
		 const CPointsMap	*buildAuxPointsMap( CPointsMap::TInsertionOptions	*ops = NULL ) const;

		 /** Copy
		   */
		 CSensoryFrame&  operator =( const CSensoryFrame &o);

		 /** Destructor.
		  */
		 virtual ~CSensoryFrame();

		 /** Clear all current observations, also freeing them.
		  */
		 void		clear();

		 /** Dump all observations in a metric map. It calls CObservation::insertObservationInto for all stored observation.
		  * \param theMap The map where this observation is to be inserted: the map will be updated.
		  * \param robotPose The pose of the robot base for this observation, relative to the target metric map. Set to NULL (default) to use (0,0,0deg)
		  *
		  * \return Returns true if the map has been updated, or false if this observations
		  *			has nothing to do with a metric map (for example, a sound observation).
		  *
		  * \sa CObservation::insertObservationInto, CMetricMap::insertObservation
		  */
		 bool  insertObservationsInto( CMetricMap *theMap, const CPose3D *robotPose = NULL ) const;

		 /** Returns the average of "likelihoodWith" evaluated between all the observations of this and another sensoryframe.
		  *
		  * \param anotherSF The other observations to compute likelihood with.
		  * \param anotherSFPose If known, the belief about the relative robot pose when the other observations were taken can be supplied here, or NULL if it is unknown.
		  *
		  * \return Returns a likelihood measurement, in the range [0,1].
		  *	\exception std::exception On any error, as another observation being of an invalid class.
		  */
		 float  likelihoodWith( const CSensoryFrame *anotherSF, const CPosePDF *anotherSFPose = NULL ) const;

		 /** You can use "sf1+=sf2;" to add observations in sf2 to sf1. Objects are copied, not referenced, thus the source can be safely deleted next.
		   * \sa moveFrom
		  */
		 void operator += (const CSensoryFrame &sf);

		 /** You can use "sf+=obs;" to add the observation "obs" to the "sf1". Objects are copied, not referenced, thus the source can be safely deleted next.
		   * \sa moveFrom
		  */
		 void operator += (const CObservation *obs);

		 /** Copies all the observation from another object, then erase them from the origin object (this method is fast since only pointers are copied).
		   * \sa operator +=
		   */
		 void moveFrom(CSensoryFrame &sf);

		 /** Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the passed object, this class will do at destructor or when appropriate.
		   */
		 void push_back(CObservation *obs);

		 /** Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the passed object, this class will do at destructor or when appropriate.
		   */
		 void insert(CObservation *obs);

		 /** Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such observation in the array.
		   * By default (ith=0), the first observation is returned.
		   */
		 CObservation*	getObservationByClass( const UTILS::TRuntimeClassId	*classID, const size_t &ith = 0 );

		 /** Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such observation in the array.
		   * By default (ith=0), the first observation is returned.
		   */
		 const CObservation*	getObservationByClass( const UTILS::TRuntimeClassId	*classID, const size_t &ith =0 ) const;

		/** You can use CSensoryFrame::begin to get a iterator to the first element.
		  */
		typedef std::deque<CObservation*>::iterator		iterator;

		/** You can use CSensoryFrame::begin to get a iterator to the first element.
		  */
		typedef std::deque<CObservation*>::const_iterator	const_iterator;

		/** Returns a iterator to the first observation: this is an example of usage:
		  * \code
		  *   CSensoryFrame  sf;
		  *   ...
		  *   for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();it++)
		  *	  {
		  *      (*it)->... // (*it) is a "CObservation*"
		  *   }
		  *
		  * \endcode
		  */
		const_iterator begin() const { return m_observations.begin(); }

		/** Returns a iterator to the end of the list of observations: this is an example of usage:
		  * \code
		  *   CSensoryFrame  sf;
		  *   ...
		  *   for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();it++)
		  *	  {
		  *      (*it)->... // (*it) is a "CObservation*"
		  *   }
		  *
		  * \endcode
		  */
		const_iterator end() const { return m_observations.end(); }

		/** Returns a iterator to the first observation: this is an example of usage:
		  * \code
		  *   CSensoryFrame  sf;
		  *   ...
		  *   for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();it++)
		  *	  {
		  *      (*it)->... // (*it) is a "CObservation*"
		  *   }
		  *
		  * \endcode
		  */
		iterator begin() { return m_observations.begin(); }

		/** Returns a iterator to the end of the list of observations: this is an example of usage:
		  * \code
		  *   CSensoryFrame  sf;
		  *   ...
		  *   for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();it++)
		  *	  {
		  *      (*it)->... // (*it) is a "CObservation*"
		  *   }
		  *
		  * \endcode
		  */
		iterator end() { return m_observations.end(); }


		/** Returns the number of observations in the list.
		  */
		size_t size() const;

		/** Removes the i'th observation in the list (0=first).
		  */
		void   eraseByIndex(const size_t &idx);

		/** Removes the given observation in the list, and return an iterator to the next element (or this->end() if it was the last one).
		  */
		iterator erase( const iterator &it);

		/** Returns the i'th observation in the list (0=first).
		  * \sa begin, size
		  */
		const CObservation* getObservationByIndex( const size_t &idx ) const;

		/** Returns the i'th observation in the list (0=first).
		  * \sa begin, size
		  */
		CObservation* getObservationByIndex( const size_t &idx );

		/** Returns the i'th observation in the list with the given "sensorLabel" (0=first).
		  * \return The observation, or NULL if not found.
		  * \sa begin, size
		  */
		const CObservation* getObservationBySensorLabel( const std::string &label, const size_t &idx = 0) const;

		/** Returns the i'th observation in the list with the given "sensorLabel" (0=first).
		  * \return The observation, or NULL if not found.
		  * \sa begin, size
		  */
		CObservation* getObservationBySensorLabel( const std::string &label, const size_t &idx = 0);

	protected:
		 /** The set of observations taken at the same time instant. See the top of this page for instructions on accessing this.
		  */
		 std::deque<CObservation*>	m_observations;

	}; // End of class def.

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


} // End of namespace

#endif
