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

#include <MRPT/UTILS/CDebugOutputCapable.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CConfigFileBase.h>
#include <MRPT/UTILS/CLoadableOptions.h>
#include <MRPT/UTILS/COpenGLScene.h>
#include <MRPT/UTILS/CKalmanFilterCapable.h>

#include <MRPT/UTILS/safe_pointers.h>

#include <MRPT/MRML/CSensoryFrame.h>
#include <MRPT/MRML/CActionCollection.h>
#include <MRPT/MRML/CObservationBearingRange.h>
#include <MRPT/MRML/CPoint3D.h>
#include <MRPT/MRML/CPose3DPDFGaussian.h>
#include <MRPT/MRML/CLandmark.h>
#include <MRPT/MRML/CSensFrameProbSequence.h>
#include <MRPT/MRML/CIncrementalMapPartitioner.h>

#include <map>

namespace MRML
{
	/** A simple implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.
      *  The main method is "processActionObservation" which processes pairs of action/observation.
      *
      *   The following Wiki page describes an front-end application based on this class:
	  *     http://babel.isa.uma.es/mrpt/index.php/Application:kf-slam
      *
	  */
	class  CRangeBearingKFSLAM : public UTILS::CDebugOutputCapable, public UTILS::CKalmanFilterCapable
	{
	 public:
		 /** Constructor.
		   */
		 CRangeBearingKFSLAM( );

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

		/** Process one new action and observations to update the map and robot pose estimate. See the description of the class at the top of this page.
		 *  \param action May contain odometry
		 *	\param SF The set of observations, must contain at least one CObservationBearingRange
		 */
		void  processActionObservation(
            const CActionCollection &action,
			const CSensoryFrame     &SF );

		/** Returns the complete mean and cov.
		  *  \param out_robotPose The mean & 6x6 covariance matrix of the robot 6D pose
		  *  \param out_landmarksPositions One entry for each of the M landmark positions (3D).
		  *  \param out_landmarkIDs Each element[index] (for indices of out_landmarksPositions) gives the corresponding landmark ID.
		  *  \param out_fullState The complete state vector (6+3M).
		  *  \param out_fullCovariance The full (6+3M)x(6+3M) covariance matrix of the filter.
		  * \sa getCurrentRobotPose
		  */
		void  getCurrentState(
			CPose3DPDFGaussian &out_robotPose,
            std::vector<CPoint3D>  &out_landmarksPositions,
            std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
            CVectorDouble      &out_fullState,
            CMatrixDouble      &out_fullCovariance
            ) const;

		/** Returns the mean & 6x6 covariance matrix of the robot 6D pose.
		  * \sa getCurrentState
		  */
		void  getCurrentRobotPose(
			CPose3DPDFGaussian &out_robotPose ) const;

		/** Returns a 3D representation of the landmarks in the map and the robot 3D position according to the current filter state.
		  *  \param out_objects
		  */
        void  getAs3DObject( UTILS::OPENGL::CSetOfObjects	&outObj ) const;

        /** Load options from a ini-like file/text
          */
        void loadOptions( const CConfigFileBase &ini );

		/** The options for the algorithm
		  */
		struct TOptions : UTILS::CLoadableOptions
		{
			/** Default values
			  */
			TOptions();

			/** Load from a config file/text
			  */
			void loadFromConfigFile(
				const CConfigFileBase	&source,
				const std::string		&section);

			/** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
			*/
			void  dumpToTextStream( CStream		&out);

			/** A 6-length vector with the std. deviation of the transition model in (x,y,z,yaw,pitch,roll) used only when there is no odometry (if there is odo, its uncertainty values will be used instead); x y z: In meters, yaw pitch roll: radians (but in degrees when loading from a configuration ini-file!)
			  */
			vector_float stds_Q_no_odo;

			/** The std. deviation of the sensor (for the matrix R in the kalman filters), in meters and radians.
			  */
            float std_sensor_range, std_sensor_yaw, std_sensor_pitch;

			/** Additional std. dev. to sum to the motion model in the z axis (useful when there is only 2D odometry and we want to put things hard to the algorithm) (default=0)
			  */
			float std_odo_z_additional;

			/** If set to true (default=false), map will be partitioned using the method stated by partitioningMethod
			  */
			bool doPartitioningExperiment;

			/** Default = 3
			  */
			float quantiles_3D_representation;

			/** Applicable only if "doPartitioningExperiment=true".
			  *   0: Automatically detect partition through graph-cut.
			  *   N>=1: Cut every "N" observations.
			  */
			int  partitioningMethod;

		} options;

		/** Return the last partition of the sequence of sensoryframes (it is NOT a partition of the map!!)
		  *  Only if options.doPartitioningExperiment = true
		  * \sa getLastPartitionLandmarks
		  */
		void getLastPartition( std::vector<vector_uint>	&parts )
		{
			parts = m_lastPartitionSet;
		}

		/** Return the partitioning of the landmarks in clusters accoring to the last partition.
		  *  Note that the same landmark may appear in different clusters (the partition is not in the space of landmarks)
		  *  Only if options.doPartitioningExperiment = true
		  *  \param landmarksMembership The i'th element of this vector is the set of clusters to which the i'th landmark in the map belongs to (landmark index != landmark ID !!).
		  * \sa getLastPartition
		  */
		void getLastPartitionLandmarks( std::vector<vector_uint>	&landmarksMembership ) const;

		/** For testing only: returns the partitioning as "getLastPartitionLandmarks" but as if a fixed-size submaps (size K) were have been used.
		  */
        void getLastPartitionLandmarksAsIfFixedSubmaps( size_t K, std::vector<vector_uint>	&landmarksMembership );


        /** Computes the ratio of the missing information matrix elements which are ignored under a certain partitioning of the landmarks.
          * \sa getLastPartitionLandmarks, getLastPartitionLandmarksAsIfFixedSubmaps
          */
        double computeOffDiagonalBlocksApproximationError( const std::vector<vector_uint>	&landmarksMembership ) const;


		/** The partitioning of the entire map is recomputed again.
		  *  Only when options.doPartitioningExperiment = true.
		  *  This can be used after changing the parameters of the partitioning method.
		  *  After this method, you can call getLastPartitionLandmarks.
		  * \sa getLastPartitionLandmarks
		  */
		void reconsiderPartitionsNow();


		/** Provides access to the parameters of the map partitioning algorithm.
		  */
		CIncrementalMapPartitioner::TOptions * mapPartitionOptions()
		{
			return &mapPartitioner.options;
		}

		/** Save the current state of the filter (robot pose & map) to a MATLAB script which displays all the elements in 2D
		  */
		void saveMapAndPath2DRepresentationAsMATLABFile(
			const std::string &fil,
			float              stdCount=3.0f,
			const std::string &styleLandmarks = std::string("b"),
			const std::string &stylePath = std::string("r"),
			const std::string &styleRobot = std::string("r") ) const;



	 protected:

		/** @name Virtual methods for Kalman Filter implementation
			@{
		 */

		/** Must return the action vector u.
		  * \param out_u The action vector which will be passed to OnTransitionModel
		  */
		void OnGetAction( CVectorTemplate<KFTYPE> &out_u );

		/** Implements the transition model \f$ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) \f$
		  * \param in_u The vector returned by OnGetAction.
		  * \param inout_x At input has \f[ \hat{x}_{k-1|k-1} \f] , at output must have \f$ \hat{x}_{k|k-1} \f$ .
		  * \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false
		  */
		void OnTransitionModel(
			const CVectorTemplate<KFTYPE>& in_u,
			CVectorTemplate<KFTYPE>&       inout_x,
			bool &out_skipPrediction
			 );

		/** Implements the transition Jacobian \f$ \frac{\partial f}{\partial x} \f$
		  * \param out_F Must return the Jacobian.
		  *  The returned matrix must be \f$N \times N\f$ with N being either the size of the whole state vector or get_vehicle_size().
		  */
		void OnTransitionJacobian(
			CMatrixTemplateNumeric<KFTYPE> & out_F
			 );

		/** Implements the transition noise covariance \f$ Q_k \f$
		  * \param out_Q Must return the covariance matrix.
		  *  The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
		  */
		void OnTransitionNoise(
			CMatrixTemplateNumeric<KFTYPE> & out_Q
			 );

		/** This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
		  * \param out_z A \f$N \times O\f$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
		  * \param out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
		  * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
		  *  This method will be called just once for each complete KF iteration.
		  */
		void OnGetObservations(
			CMatrixTemplateNumeric<KFTYPE> &out_z,
			CVectorTemplate<KFTYPE>        &out_R2,
			vector_int                     &out_data_association
			);

		/** This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
		  * \param out_z A \f$N \times O\f$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
		  * \param out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
		  * \param out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration.
		  *  This method will be called just once for each complete KF iteration.
		  *  \note Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2.
		  */
		void OnGetObservations(
			CMatrixTemplateNumeric<KFTYPE> &out_z,
			CMatrixTemplateNumeric<KFTYPE> &out_R,
			vector_int                     &out_data_association
			)
		{
			CKalmanFilterCapable::OnGetObservations(out_z,out_R,out_data_association);
		}


		/** Implements the observation prediction \f$ h_i(x) \f$ and the Jacobians \f$ \frac{\partial h_i}{\partial x} \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
		  * \param in_z This is the same matrix returned by OnGetObservations(), passed here for reference.
		  * \param in_data_association The vector returned by OnGetObservations(), passed here for reference.
		  * \param in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
		  * \param in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
		  * \param out_innov The difference between the expected observation and the real one: \f$ \tilde{y} = z - h(x) \f$. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
		  * \param out_Hx One or a vertical stack of \f$ \frac{\partial h_i}{\partial x} \f$.
		  * \param out_Hy An empty matrix, or one or a vertical stack of \f$ \frac{\partial h_i}{\partial y_i} \f$.
		  *
		  *  out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked \f$O \times V\f$ matrices, and out_Hy must consist of 1 or N \f$O \times F\f$ matrices, with:
		  *  - N: number of rows in in_z (number of observed features, or 1 in general).
		  *  - O: get_observation_size()
		  *  - V: get_vehicle_size()
		  *  - F: get_feature_size()
		  */
		void OnObservationModelAndJacobians(
			const CMatrixTemplateNumeric<KFTYPE> &in_z,
			const vector_int                     &in_data_association,
			const bool                           &in_full,
			const int                            &in_obsIdx,
			CVectorTemplate<KFTYPE>       		 &out_innov,
			CMatrixTemplateNumeric<KFTYPE>       &out_Hx,
			CMatrixTemplateNumeric<KFTYPE>       &out_Hy
			);

		/** If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
		  * \param in_z This is the same matrix returned by OnGetObservations().
		  * \param in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
		  * \param in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
		  * \param out_yn The F-length vector with the inverse observation model \f$ y_n=y(x,z_n) \f$.
		  * \param out_dyn_dxv The \f$F \times V\f$ Jacobian of the inv. sensor model wrt the robot pose \f$ \frac{\partial y_n}{\partial x_v} \f$.
		  * \param out_dyn_dhn The \f$F \times O\f$ Jacobian of the inv. sensor model wrt the observation vector \f$ \frac{\partial y_n}{\partial h_n} \f$.
		  *
		  *  - O: get_observation_size()
		  *  - V: get_vehicle_size()
		  *  - F: get_feature_size()
		  */
		void  OnInverseObservationModel(
			const CMatrixTemplateNumeric<KFTYPE> &in_z,
			const size_t                         &in_obsIdx,
			const size_t                         &in_idxNewFeat,
			CVectorTemplate<KFTYPE>              &out_yn,
			CMatrixTemplateNumeric<KFTYPE>       &out_dyn_dxv,
			CMatrixTemplateNumeric<KFTYPE>       &out_dyn_dhn );


		/** This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.
		  */
		void OnNormalizeStateVector();

		/** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
		 */
		size_t get_vehicle_size()  const { return 6; /* x y z yaw pitch roll*/ }

		/** Must return the dimension of the features in the system state (the "map"), or 0 if not applicable.
		 */
		size_t get_feature_size()  const { return 3; /* x y z */ }

		/** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
		 */
		size_t get_observation_size()  const { return 3; /* range yaw pitch */ }

		/** @}
		 */

		/** Set up by processActionObservation
		  */
		copiable_NULL_ptr<const CActionCollection>	m_action;

		/** Set up by processActionObservation
		  */
		copiable_NULL_ptr<const CSensoryFrame>		m_SF;

		/** The mapping between landmark IDs and indexes in the Pkk cov. matrix:
		  */
		std::map<CLandmark::TLandmarkID,unsigned int>	m_IDs;

		/** The mapping between indexes in the Pkk cov. matrix and landmark IDs:
		  */
		std::map<unsigned int,CLandmark::TLandmarkID>	m_IDs_inverse;


		/** Used for map partitioning experiments
		  */
		CIncrementalMapPartitioner  mapPartitioner;

		/** The sequence of all the observations and the robot path (kept for debugging, statistics,etc)
		  */
		CSensFrameProbSequence      m_SFs;

		std::vector<vector_uint>	m_lastPartitionSet;

		/** The EKF algorithm executor
		  */
		//UTILS::EKF	 *m_ekf;


	}; // end class
}
#endif
