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

#include <MRPT/MRML/CPointPDF.h>
#include <MRPT/MRML/CPointPDFGaussian.h>
#include <MRPT/UTILS/CMatrix.h>
#include <MRPT/UTILS/CMatrixD.h>


namespace UTILS
{
	namespace OPENGL
	{
		class CSetOfObjects;
	}
}

namespace MRML
{
	/** Declares a class that represents a Probability Density function (PDF) of a 3D point \f$ p(\mathbf{x}) = [x ~ y ~ z ]^t \f$.
	 *   This class implements that PDF as the following multi-modal Gaussian distribution:
	 *
	 * \f$ p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i )  \f$
	 *
	 *  Where the number of modes N is the size of CPointPDFSOG::m_modes
	 *
	 *  See MRML::CPointPDF for more details.
     *
	 * \sa CPointPDF, CPosePDF,
	 */
	class CPointPDFSOG : public CPointPDF
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CPointPDFSOG )

	protected:
		/** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
		  */
		void  assureSymmetry();

	 public:
		/** Default constructor
		  * \param nModes The initial size of CPointPDFSOG::m_modes
		  */
		CPointPDFSOG( size_t nModes = 1 );

		/** The struct for each mode:
		 */
		struct TGaussianMode
		{
			TGaussianMode() : val(), log_w(0)
			{
			}

			CPointPDFGaussian		val;

			/** The log-weight
			  */
			double		log_w;
		};

		//typedef std::deque<TGaussianMode> CListGaussianModes;
		typedef std::vector<TGaussianMode> CListGaussianModes;

		/** Access directly to this array for modify the modes as desired.
		  *  Note that no weight can be zero!!
		  */
		CListGaussianModes	m_modes;

		/** Returns the number of Gaussian modes in the SOG.
		  */
		size_t  size() const
		{
			return m_modes.size();
		}

		 /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF)
		  */
		CPoint3D  getEstimatedPoint() const;

		 /** Returns an estimate of the pose covariance matrix (3x3 cov.matrix  for x,y,z variables)
		  */
		CMatrixD  getEstimatedCovariance() const;

		/** Normalize the weights in m_modes such as the maximum log-weight is 0.
		  */
		void  normalizeWeights();

		/** Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights of the individual Gaussian modes, as a measure of the equality of the modes (in the range [0,total # of modes]).
		  */
		double ESS() const;

		/** Copy operator, translating if necesary (for example, between particles and gaussian representations)
		  */
		void  copyFrom(const CPointPDF &o);

		/** Save the density to a text file, with the following format:
		  *  There is one row per Gaussian "mode", and each row contains 10 elements:
		  *   - w (The weight)
		  *   - x_mean (gaussian mean value)
		  *   - y_mean (gaussian mean value)
		  *   - x_mean (gaussian mean value)
		  *   - C11 (Covariance elements)
		  *   - C22 (Covariance elements)
		  *   - C33 (Covariance elements)
		  *   - C12 (Covariance elements)
		  *   - C13 (Covariance elements)
		  *   - C23 (Covariance elements)
		  *
		 */
		void  saveToTextFile(const std::string &file) const;

		/** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
		  *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
		  */
		void  changeCoordinatesReference(const CPose3D &newReferenceBase );

		/** Returns a 3D object representation of all the ellipses in the SoG.
		  */
		void  getAs3DObject( UTILS::OPENGL::CSetOfObjects	&outObj ) const;

		/** Draw a sample from the pdf.
		  */
		void drawSingleSample(CPoint3D  &outSample) const;

		/** Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
		  * \param p1 The first distribution to fuse
		  * \param p2 The second distribution to fuse
		  * \param minMahalanobisDistToDrop If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.
		  */
		void  bayesianFusion( CPointPDF &p1, CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);


		/** Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).
		  */
		void  evaluatePDFInArea(
			float		x_min,
			float		x_max,
			float		y_min,
			float		y_max,
			float		resolutionXY,
			float		z,
			CMatrixD	&outMatrix,
			bool		sumOverAllZs = false );

		/** Evaluates the PDF at a given point.
		  */
		double  evaluatePDF(
			const	CPoint3D &x,
			bool	sumOverAllZs ) const;


	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CPointPDFSOG  )

} // End of namespace

#endif
