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

#include <MRPT/UTILS/CDynamicGrid.h>
#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CMatrixD.h>
#include <MRPT/UTILS/CLoadableOptions.h>
#include <MRPT/MRML/CMetricMap.h>

namespace MRML
{
	class CObservation;
	class CPose2D;

	/** The contents of each cell in a CGasConcentrationGridMap2D map.
	 **/
	struct TGasConcentrationCell
	{
		/** Constructor
		  */
		TGasConcentrationCell(
			float Mean = 0.5f,
			float Std = 0,
			float W = 1e-20f,
			float Wr = 0) : mean(Mean), std(Std),w(W),wr(Wr)
		{
		}

		/** The mean and std values of the gas concentration for this cell.
		  */
		float	mean, std;

		/** The accumulated weight and weighted-readings (See Achim's references)
		  */
		float	w,wr;
	};

	/** CGasConcentrationGridMap2D represents a PDF of gas concentrations over a 2D area.
	 **/
	class CGasConcentrationGridMap2D : public CMetricMap, public UTILS::CDynamicGrid<TGasConcentrationCell>
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CGasConcentrationGridMap2D )
	public:

		/** The type of map representation to be used.
		  */
		enum TMapRepresentation
		{
			mrAchim = 0,
			mrKalmanFilter
		};


		/** Constructor
		  */
		CGasConcentrationGridMap2D(
			TMapRepresentation	mapType = mrAchim,
            float				x_min = -2,
			float				x_max = 2,
			float				y_min = -2,
			float				y_max = 2,
			float				resolution = 0.1
			);

		 /** Erase all the contents of the map
		  */
		 void  clear();

		 /** Returns true if the map is empty/no observation has been inserted.
		   */
		 bool  isEmpty() const;

		 /** Insert the observation information into this map. This method must be implemented
		  *    in derived classes.
		  * \param obs The observation
		  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
		  *
		  * \sa CObservation::insertObservationInto
		  */
		 bool  insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );

		/** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
		 *
		 * \param takenFrom The robot's pose the observation is supposed to be taken from.
		 * \param obs The observation.
		 * \return This method returns a likelihood in the range [0,1].
		 *
		 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
		 */
		 double	 computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom );

		/** Save the current map as a graphical file (BMP,PNG,...).
		  * The file format will be derived from the file extension (see  CMRPTImage::saveToFile )
		  *  It depends on the map representation model:
		  *		mrAchim: Each pixel is the ratio \f$ \sum{\frac{wR}{w}} \f$
		  *		mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell.
		  */
		void  saveAsBitmapFile(const std::string  &filName) const;

		/** Parameters related with inserting observations into the map:
		  */
		struct TInsertionOptions : public UTILS::CLoadableOptions
		{
			/** Default values loader:
			  */
			TInsertionOptions();

			/** See UTILS::CLoadableOptions
			  */
			void  loadFromConfigFile(
				const UTILS::CConfigFileBase  &source,
				const std::string &section);

			/** See UTILS::CLoadableOptions
			  */
			void  dumpToTextStream(UTILS::CStream		&out);

			/** The sigma of the "Parzen"-kernel Gaussian
			  */
			float	sigma;

			/** The cutoff radius for updating cells.
			  */
			float	cutoffRadius;

			/** Limits for normalization of sensor readings.
			  */
            float	R_min,R_max;

			/** [KF model] The "sigma" for the initial covariance value between cells (in meters).
			  */
			float	KF_covSigma;

			/** [KF model] The initial standard deviation of each cell's concentration (will be stored both at each cell's structure and in the covariance matrix as variances in the diagonal) (in normalized concentration units).
			  */
			float	KF_initialCellStd;

			/** [KF model] The sensor model noise (in normalized concentration units).
			  */
			float	KF_observationModelNoise;

			/** [KF model] The default value for the mean of cells' concentration.
			  */
			float	KF_defaultCellMeanValue;

		} insertionOptions;

		/** Changes the size of the grid, maintaining previous contents.
		  * \sa setSize
		  */
		void  resize(		float	new_x_min,
									float	new_x_max,
									float	new_y_min,
									float	new_y_max,
									const TGasConcentrationCell& defaultValueNewCells,
									float	additionalMarginMeters = 1.0f );

		/** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
		 *   In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
		 * \param  otherMap					  [IN] The other map to compute the matching with.
		 * \param  otherMapPose				  [IN] The 6D pose of the other map as seen from "this".
		 * \param  minDistForCorr			  [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
		 * \param  minMahaDistForCorr		  [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
		 *
		 * \return The matching ratio [0,1]
		 * \sa computeMatchingWith2D
		 */
		float  compute3DMatchingRatio(
				const CMetricMap						*otherMap,
				const CPose3D							&otherMapPose,
				float									minDistForCorr = 0.10f,
				float									minMahaDistForCorr = 2.0f
				) const;


		/** The implementation in this class just calls all the corresponding method of the contained metric maps.
		  */
		void  saveMetricMapRepresentationToFile(
			const std::string	&filNamePrefix
			) const;

		/** Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell.
		  *  This method can only be called in a KF map model.
		  */
		void  saveAsMatlab3DGraph(const std::string  &filName) const;

		/** Returns a 3D object representing the map.
		  */
		void  getAs3DObject ( UTILS::OPENGL::CSetOfObjects	&outObj ) const;

		/** This method is called at the end of each "prediction-update-map insertion" cycle within "MRML::CMetricMapBuilderRBPF::processActionObservation".
		  *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
		  */
		void  auxParticleFilterCleanUp();

		/** Return the type of the gas distribution map, according to parameters passed on construction.
		  */
		TMapRepresentation	 getMapType();

	protected:

		/** The map representation type of this map.
		  */
		TMapRepresentation		m_mapType;

		/** The whole covariance matrix, used for the Kalman Filter map representation.
		  */
		CMatrixD				m_cov;

		/** The implementation of "insertObservation" for the Achim's map model.
		  * \param normReading Is a [0,1] normalized concentration reading.
		  * \param sensorPose Is the sensor pose
		  */
		void  insertObservation_Achim(
			float			normReading,
			const CPose3D	&sensorPose );

		/** The implementation of "insertObservation" for the (whole) Kalman Filter map model.
		  * \param normReading Is a [0,1] normalized concentration reading.
		  * \param sensorPose Is the sensor pose
		  */
		void  insertObservation_KF(
			float			normReading,
			const CPose3D	&sensorPose );

		/** Computes the average cell concentration, or 0 if it has never been observed:
		  */
		static float computeAchimCellValue (const TGasConcentrationCell *cell );

	};

	DEFINE_SERIALIZABLE_POST( CGasConcentrationGridMap2D )

} // End of namespace

#endif
