/*---------------------------------------------------------------
	FILE: CHierarchicalMappingFramework.h
	USE: See doxygen doc.

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

#include <MRPT/UTILS/SYNCH.h>
#include <MRPT/UTILS/CParticleFilterCapable.h>
#include <MRPT/MRML/CHierarchicalMap.h>
#include <MRPT/MRML/CActionCollection.h>
#include <MRPT/MRML/CIncrementalMapPartitioner.h>
#include <MRPT/MRML/CPosePDFParticles.h>
#include <MRPT/MRML/CPosePDFGaussian.h>

namespace MRML
{

	/** This class encapsulates a powerful hierarchical mapping framework (see related papers for details)
	 *
	 *	NOTES:<br>
	 *		- Methods marked as "THREAD SAFE" internally use the critical sections, thus user MUST NOT acquire them prior to calling, or a deadlock will occur.
	 *		- This class is the first implementation of the new approach to SLAM originally reported in:
	 *			- "A New Approach for Large-Scale Localization and Mapping: Hybrid Metric-Topological SLAM", in ICRA 2007.
	 *		- A more powerful implementation can be found in the class "MRML::..."
	 *
	 * \sa CHierarchicalMap,CHMapNode
	 */
	class CHierarchicalMappingFramework : public CDebugOutputCapable
	{
	protected:
		class CRBPFData;
		friend class CHierarchicalMappingFramework::CRBPFData;

		typedef std::vector<CHMapNode::THMapNodeID> TTopologicalLoopClosure;

		/*-------------------------------------------------------------
							PARTICLES
		  ------------------------------------------------------------*/
		/** The data associated to each particle in the low-level metric SLAM in MRML::CHierarchicalMappingFramework
		  */
		class CRBPFData
		{
		public:
			/** Default constructor
			  */
			CRBPFData( MRML::CHierarchicalMappingFramework	*parent );

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

			/** Destructor
			  */
			virtual ~CRBPFData();

			/** The ID of the area node in the hierarchical map that is used as coordinates reference in this particle.
			  */
            CHMapNode::THMapNodeID 					m_areaID;

			// Add topolog.loop close hypotesis here????

			/** Only the poses corresponding to the insertion of observations into the map are saved here, excepting the lastest robot pose, which is always the last one in the path.
			  */
			std::deque<CPose2D>						m_robotPath;

			CMultiMetricMap							*m_mapTillNow;
			MRML::CHierarchicalMappingFramework		*m_parent;
		};


		/** The default set of maps to be created in each particle:
		  */
		TSetOfMetricMapInitializers					m_defaultMapsInitializers;

		/** The default options for the CMultiMetricMap in each particle.
		  */
		CMultiMetricMap::TOptions					m_defaultMapsOptions;

	public:
		/** An internal class that represents the low-level RBPF for SLAM.
		  */
		class CRBPF : public CParticleFilterCapable, public CParticleFilterData<CRBPFData>
		{
			// This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
			IMPLEMENT_PARTICLE_FILTER_CAPABLE(CRBPFData);

		private:
			MRML::CHierarchicalMappingFramework		*m_parent;

			/** Clears the "localMetricMap_SFs" list and frees memory.
			  */
			void  clear_localMetricMap_SFs();

		public:

			/** States if the last robot pose in the particle paths was an inserted observation or not (in this case the pose will be overwritten in the next prediction step).
			  */
			bool	m_lastPathStepWasInserted;

			/** For use in the "localMetricMap_SFs"
			  */
			struct TLocalizedObservation
			{
				CSensorialFrame		*sf;
				CPosePDF			*posePDF;
				// ...
			};

			/** The localized-observations that build the "current local metric map"
			  */
			std::deque<TLocalizedObservation>	localMetricMap_SFs;

			/** Default constructor
			  */
			CRBPF( MRML::CHierarchicalMappingFramework *parent );

			/** Destructor
			  */
			~CRBPF();

			/** Initialize the particle filter with M default-valued m_particles.
			*/
			void  initializeParticleCount( unsigned int M );

			// --- IMPLEMENTATION OF THE PARTICLE FILTER CAPABLE INTERFACE --

			/** The "optimal proposal" algorithm implementation
			  */
			void  prediction_and_update_pfOptimalProposal(
				const MRML::CActionCollection	* action,
				const MRML::CSensorialFrame		* sf );

			/** Returns a pointer to the lastest pose of a given particle in the filter.
			  */
			CPose2D	*  getLastPose(unsigned int i);

			/** Returns a pointer to the lastest pose of a given particle in the filter.
			  */
			CPose2D	*  getLastPose(CParticleData *part);

			/** Saves a debug image with the current low-level metric map in the RBPF (THREAD-SAFE)
			  */
			void  saveCurrentEstimationToBitmapFile(std::string file);

			/** Return the path (in absolute coordinate poses) for the i'th particle.
			* \exception On index out of bounds
			*/
			void  getPath(size_t i, std::deque<CPose2D> &out_path);

			/** Saves the pose m_particles to a text file for debugging (THREAD-SAFE).
			  */
			void  savePoseParticlesToTextFile(char *file);

			/** Insert an observation to the map, at each particle's pose and to each particle's metric map.
			* \param sf The SF to be inserted
			*/
			void  insertObservation(CSensorialFrame	&sf);

			/** Returns a "CPosePDFParticles" object filled out with the lastest poses of each RBPF-particle.
			  */
			void  getEstimatedPosePDF( CPosePDFParticles	&out_estimation );

			/** Returns a "CPosePDFParticles" object filled out with the poses of each RBPF-particle at a given instant of time (m_particles-associated paths)
			  */
			void  getEstimatedPosePDFAtTime(
							int					timeStep,
							CPosePDFParticles	&out_estimation );

		} *rbpf;

		friend class CRBPF;

		/*-------------------------------------------------------------
							THE MAIN THINGS
		  ------------------------------------------------------------*/
		/** The known areas ("Long-term memory" in the paper)
		  */
		CHierarchicalMap			m_knownAreas;

	private:

		/** Used in the AAM thread to find groups into the observations.
		  */
		CIncrementalMapPartitioner	m_mapPartitioner;

		/** The queue of pending actions/observations supplied by the user waiting for being processed.
		  */
		std::queue<CSerializable*>	m_inputQueue;

		/*-------------------------------------------------------------
						AUXILIARY, INTERNAL DATA AND METHODS
		  ------------------------------------------------------------*/
		/** Empty the input queue.
		  */
		void  clearInputQueue();

		/** The acumulators for robot movements (used in SLAM to determine whether to include observations into the maps)
		  */
		float	m_cumMovedLinear, m_cumMovedAngular;

		/*-------------------------------------------------------------
							THREADS STUFF
		  ------------------------------------------------------------*/
		/** The function for the "SLAM" thread.
		  */
		static void thread_SLAM( void *ptrThis );

		/** The function for the "AAM" thread.
		  */
		static void thread_AAM( void *ptrThis );

		/** The function for the "TSBI" thread.
		  */
		static void thread_TSBI( void *ptrThis );

		/** The function for the "TLCA" thread.
		  */
		static void thread_TLCA( void *ptrThis );

		/** The function for the "3D Server" thread.
		  */
		static void thread_3D_Server( void *ptrThis );

	public:
		/** Semaphores (internal use)
		  */
		UTILS::SYNCH::CCriticalSection	m_csAccessToHMap,
										m_csAccessToRBPF;
	private:
		/** Semaphores (internal use)
		  */
		UTILS::SYNCH::CCriticalSection	m_csAccessToInputQueue,
										m_csAccessAccessToTSBIQueue,
										m_csAccessAccessToTLCAQueue;

        UTILS::SYNCH::CSemaphore        m_semaphoreInputQueueHasData,
                                        m_eventNewObservationInserted;

		/** Threads handles
		  */
		unsigned long 			m_hThread_SLAM,
								m_hThread_AAM,
								m_hThread_TSBI,
								m_hThread_TLCA,
								m_hThread_3D_Server;

		/** Termination flag for threads to terminate:
		  */
		bool					m_terminateThreads;

		/** Threads termination flags:
		  */
		bool					m_terminationFlag_SLAM,
								m_terminationFlag_AAM,
								m_terminationFlag_TSBI,
								m_terminationFlag_TLCA,
								m_terminationFlag_3D_Server;

		/** The queue of pending areas for their topological correspondence to be established (TSBI thread)
		  */
		std::queue<CHMapNode::THMapNodeID>	m_TSBI_input_queue;

		/** The queue of pending topological loop-closures (TLCA thread)
		  */
		std::queue<TTopologicalLoopClosure>		m_TLCA_input_queue;

		/** Generates a new and unique area textual label (currently this generates "0","1",...)
		  */
		std::string	 generateUniqueAreaLabel();

		/** Enqueue a given node for the TSBI thread to work with it (Thread safe)
		  */
		void  enqueueNodeForTSBI( CHMapNode::THMapNodeID	nodeId );

		/** Enqueue a given topological-loop closure for the TLCA thread to work with it (Thread safe)
		  */
		void  enqueueNodeForTLCA( TTopologicalLoopClosure	&loopClosure );

	public:
		/** Default constructor
		  * \param debuggingOutStream  You can redirect debug messages to a given output stream here. If you pass NULL, messages will be send to the standard debuggig stream (default: stdout)
		  */
		CHierarchicalMappingFramework( CStream *debuggingOutStream = NULL );

		/** Destructor
		  */
		virtual ~CHierarchicalMappingFramework();

		/** Here the user can enter an action to the system (will go to the SLAM process).
		  * \sa pushObservations
		  */
		void  pushAction( CActionCollection *acts );

		/** Here the user can enter observations to the system (will go to the SLAM process).
		  * \sa pushAction
		  */
		void  pushObservations( CSensorialFrame *sf );

		/** Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)
		  * \sa pushAction,pushObservations
		  */
		bool  isInputQueueEmpty();

		/** Initializes the whole mapping framework, setting an empty map as the starting situation (It also clears the logs directory).
		  */
		void  initializeEmptyMap();

		/** Gets a 3D representation of the current state of the whole mapping framework.
		  */
		void  getAs3DScene( COpenGLScene	&outScene );

		/** A variety of options and configuration params
		  */
		struct TOptions
		{
			/** Initialization of default params
			  */
			TOptions();

			/** [LOGGING] If it is not an empty string (""), a directory with that name will be created and log files save there.
			  */
			std::string	logOutputDirectory;

            /** The options for the particle filter algorithm.
              */
			CParticleFilter::TParticleFilterOptions     PF_options;

			/** [SLAM] Number of m_particles to be used in low-level metric SLAM (RBPF).
			  */
			//unsigned int SLAM_particles_count;

			/** [SLAM] The resampling threshold in range [0,1] for the RBPF (default 0.5)
			  */
			//float		SLAM_resampling_BETA;

			/** [SLAM] The resolution of occupancy grid maps:
			  */
			float		SLAM_grid_resolution;

			/** [SLAM, Prediction] Whether to use ICP-based global localization for each particle.
			  */
			bool		SLAM_prediction_useICP;

			/** [SLAM, Update] The map to be used in the m_particles likelihood computation (0:gridmap,1:landmark-map)
			  */
			int			SLAM_update_likelihood_map;

			/** [SLAM] The thresholds of space traveled by the robot to include a new observation into the map (and reset counters)
			*/
			float		SLAM_insertionThresholdDist, SLAM_insertionThresholdAng;

			/** [AAM] The partition threshold in the range [0,2].
			*/
			float		AAM_partitionThreshold;

			/** [AAM] Minimum distance between two 2D points to consider them a correspondence (in meters).
			*/
			float		AAM_minDistCorr;

			/** [SLAM] When using the consensus likelihood method on grid maps, this represents the down-sample ratio of the laser range scans (default=5=take 1 out of 5)
			  */
			int			SLAM_likConsensus_downRatio;

			/** [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
			  */
			int			LOGGING_logFrequency;

			/** [TSBI] Threshold of the matching probability for establishing a loop closure (typ.0.15-0.3)
			  */
			float		TSBI_loopClosureThreshold;

			/** [Various] Parameters for the occupancy grid, as loaded from the INI file by "loadOptions".
			  */
			COccupancyGridMap2D::TInsertionOptions		occGridInsertionOpts;

			/** [SLAM] Parameters for the occupancy grid likelihood computation, as loaded from the INI file by "loadOptions".
			  */
			COccupancyGridMap2D::TLikelihoodOptions		occGridLikelihoodOpts;

		} options;

		/** Loads the options from a config file.
		  * \return Returns true if all go ok, false on any error.
		  */
		bool  loadOptions( const std::string &configFile );

		/** Forces a synchronization between the low-level RBPF SLAM and the HMap of "Areas" (e.g. call this after finishing a mapping experiment to assure areas are updated) (THREAD SAFE).
		  */
		void  updateHierarchicalMapFromRBPF(bool onlySFs = false);

		/** Returns a read-only reference to the so long built hierarchical map:
		  */
		void  getCurrentlyBuiltHMap( CHierarchicalMap		&outMapCopy,
											   unsigned int			&outCurrentMostLikelyAreaIndex,
											   CPosePDFParticles	&outCurrentRobotPose );

	}; // End of class def.

} // End of namespace

#endif
