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

#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <MRPT/UTILS/CLoadableOptions.h>

namespace UTILS
{

	/** The Kalman Filter algorithm to employ in UTILS::CKalmanFilterCapable
	  *  For further details on each algorithm see the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters
	  * \sa UTILS::CKalmanFilterCapable::KF_options
	  */
	enum TKFMethod {
		kfEKFNaive = 0,
		kfEKFAlaDavison,
		kfIKFFull,
		kfIKF
	};

	/** The type of all vectors and matrices in CKalmanFilterCapable
	  */
	typedef double KFTYPE;

    /** Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
     *   This base class stores the state vector and covariance matrix of the system. It has virtual methods that must be completed
     *    by derived classes to address a given filtering problem. The main entry point of the algorithm is CKalmanFilterCapable::runOneKalmanIteration, which
     *    should be called AFTER setting the desired filter options in KF_options, as well as any options in the derived class.
     *   Note that the main entry point is protected, so derived classes must offer another method more specific to a given problem which, internally, calls runOneKalmanIteration.
     *
     *  For further details and examples, check out the tutorial: http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters
     *
	 *  \note Since MRPT 0.5.5 there are two  methods "OnGetObservations": one returning the sensor noise covariance matrix as a whole matrix, and the other as the diagonal vector. The derived class must implement just one of them.
     *
	 * Revisions:
	 *	- 2007: Antonio J. Ortiz de Galisteo (AJOGD)
	 *	- 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
	 *	- 2008/MAR: Implemented IKF (JLBC).
     *
     *  \sa MRML::CRangeBearingKFSLAM
     */
	class CKalmanFilterCapable
	{
	protected:
		/** The system state vector.
		  */
		CVectorTemplate<KFTYPE> 		m_xkk;

		/** The system full covariance matrix.
		  */
		CMatrixTemplateNumeric<KFTYPE> 	m_pkk;

		/** The main entry point, executes one complete step: prediction + update.
		  *  It is protected since derived classes must provide a problem-specific entry point for users.
		  *  The exact order in which this method calls the virtual method is explained in http://babel.isa.uma.es/mrpt/index.php/Kalman_Filters.
		  */
		void runOneKalmanIteration();

		/** @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
		  */
		virtual void OnGetAction( CVectorTemplate<KFTYPE> &out_u ) = 0;

		/** 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
		  */
		virtual void OnTransitionModel(
			const CVectorTemplate<KFTYPE>& in_u,
			CVectorTemplate<KFTYPE>&       inout_x,
			bool &out_skipPrediction
			 ) = 0;

		/** 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().
		  */
		virtual void OnTransitionJacobian(
			CMatrixTemplateNumeric<KFTYPE> & out_F
			 ) = 0;

		/** 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
		  */
		virtual void OnTransitionNoise(
			CMatrixTemplateNumeric<KFTYPE> & out_Q
			 ) = 0;

		/** 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_R2 A vector of size O (O being get_observation_size()) with the *variances* of the sensor noise for each of the observation components. This is constant for all the observations (where N>1), and in the naive Kalman method the value TKF_options::veryLargeR2 is used for unobserved map elements.
		  * \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.
		  */
		virtual void OnGetObservations(
			CMatrixTemplateNumeric<KFTYPE> &out_z,
			CVectorTemplate<KFTYPE> 	   &out_R2,
			vector_int                     &out_data_association
			)
		{
			MRPT_TRY_START
			THROW_EXCEPTION("One version of the OnGetObservations method must be implemented in the derived class.")
			MRPT_TRY_END
		}

		/** 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.
		  */
		virtual void OnGetObservations(
			CMatrixTemplateNumeric<KFTYPE> &out_z,
			CMatrixTemplateNumeric<KFTYPE> &out_R,
			vector_int                     &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()
		  */
		virtual 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
			) = 0;

		/** 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()
		  */
		virtual 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.
		  */
		virtual void OnNormalizeStateVector();

		/** This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
		  */
		virtual void OnPostIteration();


		/** Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
		 */
		virtual size_t get_vehicle_size() const = 0;

		/** Must return the dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
		 */
		virtual size_t get_feature_size() const
		{
			return 0; // The default for not SLAM-like problems.
		}

		/** Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
		 */
		virtual size_t get_observation_size() const= 0;

		/** The index of the first component of the vehicle state within the whole state vector/matrix (default=0)
		  */
		virtual size_t get_vehicle_state_offset() const
		{
			return 0;
		}

		/** The index of the first component of the features part in the state vector/matrix (default=after the vehicle state, assuming it is at the begining of the vector)
		  */
		virtual size_t get_feature_state_offset() const
		{
			return get_vehicle_size();
		}

		/** @}
		 */


	public:

		/** Default constructor
		 */
		CKalmanFilterCapable();

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

		/** Generic options for the Kalman Filter algorithm in itself.
		  */
		struct TKF_options : public UTILS::CLoadableOptions
		{
			TKF_options();

			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);


			/** The method to employ (default: kfEKFAlaDavison)
			  */
			TKFMethod	method;

			/** If set to true timing information will be dumped to the console (default=false)
			  */
			bool   verbose;

			/** Value used as the diagonal of R for unobserved map elements, only in the kfEKFNaive algorithm.
			  */
			double      veryLargeR2;

			/** Number of refinement iterations, only for the IKF method.
			  */
			int 		IKF_iterations;
		};

		/** Generic options for the Kalman Filter algorithm in itself.
		  */
		TKF_options KF_options;

	}; // end class

} // end namespace

#endif //__EKF_H
