/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/containers/stl_containers_utils.h>
#include <mrpt/containers/stl_containers_utils.h>  // find_in_vector
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CVectorFixed.h>
#include <mrpt/math/num_jacobian.h>
#include <mrpt/math/utils.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/typemeta/TEnumType.h>

#include <cstring>	// memcpy
#include <vector>

#if defined(_DEBUG)
#include <mrpt/io/vector_loadsave.h>
#endif

namespace mrpt
{
namespace bayes
{
/** The Kalman Filter algorithm to employ in bayes::CKalmanFilterCapable
 *  For further details on each algorithm see the tutorial:
 * https://www.mrpt.org/Kalman_Filters
 * \sa bayes::CKalmanFilterCapable::KF_options
 * \ingroup mrpt_bayes_grp
 */
enum TKFMethod
{
	kfEKFNaive = 0,
	kfEKFAlaDavison,
	kfIKFFull,
	kfIKF
};

// Forward declaration:
template <
	size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
	typename KFTYPE>
class CKalmanFilterCapable;

/** Generic options for the Kalman Filter algorithm in itself.
 * \ingroup mrpt_bayes_grp
 */
struct TKF_options : public mrpt::config::CLoadableOptions
{
	TKF_options(mrpt::system::VerbosityLevel& verb_level_ref)
		: verbosity_level(verb_level_ref)
	{
	}

	void loadFromConfigFile(
		const mrpt::config::CConfigFileBase& iniFile,
		const std::string& section) override
	{
		method = iniFile.read_enum<TKFMethod>(section, "method", method);
		verbosity_level = iniFile.read_enum<mrpt::system::VerbosityLevel>(
			section, "verbosity_level", verbosity_level);
		MRPT_LOAD_CONFIG_VAR(IKF_iterations, int, iniFile, section);
		MRPT_LOAD_CONFIG_VAR(enable_profiler, bool, iniFile, section);
		MRPT_LOAD_CONFIG_VAR(
			use_analytic_transition_jacobian, bool, iniFile, section);
		MRPT_LOAD_CONFIG_VAR(
			use_analytic_observation_jacobian, bool, iniFile, section);
		MRPT_LOAD_CONFIG_VAR(
			debug_verify_analytic_jacobians, bool, iniFile, section);
		MRPT_LOAD_CONFIG_VAR(
			debug_verify_analytic_jacobians_threshold, double, iniFile,
			section);
	}

	/** This method must display clearly all the contents of the structure in
	 * textual form, sending it to a CStream. */
	void dumpToTextStream(std::ostream& out) const override
	{
		out << "\n----------- [TKF_options] ------------ \n\n";
		out << mrpt::format(
			"method                                  = %s\n",
			mrpt::typemeta::TEnumType<TKFMethod>::value2name(method).c_str());
		out << mrpt::format(
			"verbosity_level                         = %s\n",
			mrpt::typemeta::TEnumType<mrpt::system::VerbosityLevel>::value2name(
				verbosity_level)
				.c_str());
		out << mrpt::format(
			"IKF_iterations                          = %i\n", IKF_iterations);
		out << mrpt::format(
			"enable_profiler                         = %c\n",
			enable_profiler ? 'Y' : 'N');
		out << "\n";
	}

	/** The method to employ (default: kfEKFNaive) */
	TKFMethod method{kfEKFNaive};
	mrpt::system::VerbosityLevel& verbosity_level;
	/** Number of refinement iterations, only for the IKF method. */
	int IKF_iterations{5};
	/** If enabled (default=false), detailed timing information will be dumped
	 * to the console thru a CTimerLog at the end of the execution. */
	bool enable_profiler{false};
	/** (default=true) If true, OnTransitionJacobian will be called; otherwise,
	 * the Jacobian will be estimated from a numeric approximation by calling
	 * several times to OnTransitionModel. */
	bool use_analytic_transition_jacobian{true};
	/** (default=true) If true, OnObservationJacobians will be called;
	 * otherwise, the Jacobian will be estimated from a numeric approximation by
	 * calling several times to OnObservationModel. */
	bool use_analytic_observation_jacobian{true};
	/** (default=false) If true, will compute all the Jacobians numerically and
	 * compare them to the analytical ones, throwing an exception on mismatch.
	 */
	bool debug_verify_analytic_jacobians{false};
	/** (default-1e-2) Sets the threshold for the difference between the
	 * analytic and the numerical jacobians */
	double debug_verify_analytic_jacobians_threshold{1e-2};
};

/** Auxiliary functions, for internal usage of MRPT classes */
namespace detail
{
// Auxiliary functions.
template <
	size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
	typename KFTYPE>
inline size_t getNumberOfLandmarksInMap(
	const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>&
		obj);
// Specialization:
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
inline size_t getNumberOfLandmarksInMap(
	const CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj);

template <
	size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
	typename KFTYPE>
inline bool isMapEmpty(
	const CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>&
		obj);
// Specialization:
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
inline bool isMapEmpty(
	const CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, 0 /*FEAT_SIZE*/, ACT_SIZE, KFTYPE>& obj);

template <
	size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
	typename KFTYPE>
void addNewLandmarks(
	CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>& obj,
	const typename CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::vector_KFArray_OBS& Z,
	const std::vector<int>& data_association,
	const typename CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>::KFMatrix_OxO& R);
template <size_t VEH_SIZE, size_t OBS_SIZE, size_t ACT_SIZE, typename KFTYPE>
void addNewLandmarks(
	CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE, KFTYPE>& obj,
	const typename CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
		KFTYPE>::vector_KFArray_OBS& Z,
	const std::vector<int>& data_association,
	const typename CKalmanFilterCapable<
		VEH_SIZE, OBS_SIZE, 0 /* FEAT_SIZE=0 */, ACT_SIZE,
		KFTYPE>::KFMatrix_OxO& R);
}  // namespace detail

/** 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://www.mrpt.org/Kalman_Filters
 *
 *  The Kalman filter algorithms are generic, but this implementation is biased
 *to ease the implementation
 *  of SLAM-like problems. However, it can be also applied to many generic
 *problems not related to robotics or SLAM.
 *
 *  The meaning of the template parameters is:
 *	- VEH_SIZE: The dimension of the "vehicle state": either the full state
 *vector or the "vehicle" part if applicable.
 *	- OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates,
 *3 for 3D coordinates,etc).
 *	- FEAT_SIZE: The dimension of the features in the system state (the "map"),
 *or 0 if not applicable (the default if not implemented).
 *	- ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).
 *	- KFTYPE: The numeric type of the matrices (default: double)
 *
 * Revisions:
 *	- 2007: Antonio J. Ortiz de Galisteo (AJOGD)
 *	- 2008/FEB: All KF classes corrected, reorganized, and rewritten (JLBC).
 *	- 2008/MAR: Implemented IKF (JLBC).
 *	- 2009/DEC: Totally rewritten as a generic template using fixed-size
 *matrices where possible (JLBC).
 *
 *  \sa mrpt::slam::CRangeBearingKFSLAM, mrpt::slam::CRangeBearingKFSLAM2D
 * \ingroup mrpt_bayes_grp
 */
template <
	size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE,
	typename KFTYPE = double>
class CKalmanFilterCapable : public mrpt::system::COutputLogger
{
   public:
	static constexpr size_t get_vehicle_size() { return VEH_SIZE; }
	static constexpr size_t get_observation_size() { return OBS_SIZE; }
	static constexpr size_t get_feature_size() { return FEAT_SIZE; }
	static constexpr size_t get_action_size() { return ACT_SIZE; }
	inline size_t getNumberOfLandmarksInTheMap() const
	{
		return detail::getNumberOfLandmarksInMap(*this);
	}
	inline bool isMapEmpty() const { return detail::isMapEmpty(*this); }
	/** The numeric type used in the Kalman Filter (default=double) */
	using kftype = KFTYPE;
	/** My class, in a shorter name! */
	using KFCLASS =
		CKalmanFilterCapable<VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE>;

	// ---------- Many useful typedefs to short the notation a bit... --------
	using KFVector = mrpt::math::CVectorDynamic<KFTYPE>;
	using KFMatrix = mrpt::math::CMatrixDynamic<KFTYPE>;

	using KFMatrix_VxV = mrpt::math::CMatrixFixed<KFTYPE, VEH_SIZE, VEH_SIZE>;
	using KFMatrix_OxO = mrpt::math::CMatrixFixed<KFTYPE, OBS_SIZE, OBS_SIZE>;
	using KFMatrix_FxF = mrpt::math::CMatrixFixed<KFTYPE, FEAT_SIZE, FEAT_SIZE>;
	using KFMatrix_AxA = mrpt::math::CMatrixFixed<KFTYPE, ACT_SIZE, ACT_SIZE>;

	using KFMatrix_VxO = mrpt::math::CMatrixFixed<KFTYPE, VEH_SIZE, OBS_SIZE>;
	using KFMatrix_VxF = mrpt::math::CMatrixFixed<KFTYPE, VEH_SIZE, FEAT_SIZE>;
	using KFMatrix_FxV = mrpt::math::CMatrixFixed<KFTYPE, FEAT_SIZE, VEH_SIZE>;
	using KFMatrix_FxO = mrpt::math::CMatrixFixed<KFTYPE, FEAT_SIZE, OBS_SIZE>;
	using KFMatrix_OxF = mrpt::math::CMatrixFixed<KFTYPE, OBS_SIZE, FEAT_SIZE>;
	using KFMatrix_OxV = mrpt::math::CMatrixFixed<KFTYPE, OBS_SIZE, VEH_SIZE>;

	using KFArray_VEH = mrpt::math::CVectorFixed<KFTYPE, VEH_SIZE>;
	using KFArray_ACT = mrpt::math::CVectorFixed<KFTYPE, ACT_SIZE>;
	using KFArray_OBS = mrpt::math::CVectorFixed<KFTYPE, OBS_SIZE>;
	using vector_KFArray_OBS = std::vector<KFArray_OBS>;
	using KFArray_FEAT = mrpt::math::CVectorFixed<KFTYPE, FEAT_SIZE>;

	inline size_t getStateVectorLength() const { return m_xkk.size(); }
	inline KFVector& internal_getXkk() { return m_xkk; }
	inline KFMatrix& internal_getPkk() { return m_pkk; }
	/** Returns the mean of the estimated value of the idx'th landmark (not
	 * applicable to non-SLAM problems).
	 * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
	 */
	inline void getLandmarkMean(size_t idx, KFArray_FEAT& feat) const
	{
		ASSERT_(idx < getNumberOfLandmarksInTheMap());
		std::memcpy(
			&feat[0], &m_xkk[VEH_SIZE + idx * FEAT_SIZE],
			FEAT_SIZE * sizeof(m_xkk[0]));
	}
	/** Returns the covariance of the idx'th landmark (not applicable to
	 * non-SLAM problems).
	 * \exception std::exception On idx>= getNumberOfLandmarksInTheMap()
	 */
	inline void getLandmarkCov(size_t idx, KFMatrix_FxF& feat_cov) const
	{
		feat_cov = m_pkk.template blockCopy<FEAT_SIZE, FEAT_SIZE>(
			VEH_SIZE + idx * FEAT_SIZE, VEH_SIZE + idx * FEAT_SIZE);
	}

   protected:
	/** @name Kalman filter state
		@{ */

	/** The system state vector. */
	KFVector m_xkk;
	/** The system full covariance matrix. */
	KFMatrix m_pkk;

	/** @} */

	mrpt::system::CTimeLogger m_timLogger;

	/** @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(KFArray_ACT& out_u) const = 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
	 * \note Even if you return "out_skip=true", the value of "inout_x" MUST be
	 * updated as usual (this is to allow numeric approximation of Jacobians).
	 */
	virtual void OnTransitionModel(
		const KFArray_ACT& in_u, KFArray_VEH& inout_x,
		bool& out_skipPrediction) const = 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$V \times V\f$ with V being either the
	 * size of the whole state vector (for non-SLAM problems) or VEH_SIZE (for
	 * SLAM problems).
	 */
	virtual void OnTransitionJacobian(
		[[maybe_unused]] KFMatrix_VxV& out_F) const
	{
		m_user_didnt_implement_jacobian = true;
	}

	/** Only called if using a numeric approximation of the transition Jacobian,
	 * this method must return the increments in each dimension of the vehicle
	 * state vector while estimating the Jacobian.
	 */
	virtual void OnTransitionJacobianNumericGetIncrements(
		KFArray_VEH& out_increments) const
	{
		for (size_t i = 0; i < VEH_SIZE; i++)
			out_increments[i] = 1e-6;
	}

	/** 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(KFMatrix_VxV& out_Q) const = 0;

	/** This will be called before OnGetObservationsAndDataAssociation to allow
	 * the application to reduce the number of covariance landmark predictions
	 * to be made.
	 *  For example, features which are known to be "out of sight" shouldn't be
	 * added to the output list to speed up the calculations.
	 * \param in_all_prediction_means The mean of each landmark predictions;
	 * the computation or not of the corresponding covariances is what we're
	 * trying to determined with this method.
	 * \param out_LM_indices_to_predict The list of landmark indices in the map
	 * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
	 * \note This is not a pure virtual method, so it should be implemented
	 * only if desired. The default implementation returns a vector with all the
	 * landmarks in the map.
	 * \sa OnGetObservations, OnDataAssociation
	 */
	virtual void OnPreComputingPredictions(
		[[maybe_unused]] const vector_KFArray_OBS& in_all_prediction_means,
		std::vector<size_t>& out_LM_indices_to_predict) const
	{
		// Default: all of them:
		const size_t N = this->getNumberOfLandmarksInTheMap();
		out_LM_indices_to_predict.resize(N);
		for (size_t i = 0; i < N; i++)
			out_LM_indices_to_predict[i] = i;
	}

	/** Return the observation NOISE covariance matrix, that is, the model of
	 * the Gaussian additive noise of the sensor.
	 * \param out_R The noise covariance matrix. It might be non diagonal, but
	 * it'll usually be.
	 * \note Upon call, it can be assumed that the previous contents of out_R
	 * are all zeros.
	 */
	virtual void OnGetObservationNoise(KFMatrix_OxO& out_R) const = 0;

	/** This is called between the KF prediction step and the update step, and
	 * the application must return the observations and, when applicable, the
	 * data association between these observations and the current map.
	 *
	 * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
	 * being the number of "observations": how many observed landmarks for a
	 * map, or just one if not applicable.
	 * \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 (in
	 * the range [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map
	 * element and we want to insert it at the end of this KF iteration.
	 * \param in_all_predictions A vector with the prediction of ALL the
	 * landmarks in the map. Note that, in contrast, in_S only comprises a
	 * subset of all the landmarks.
	 * \param in_S The full covariance matrix of the observation predictions
	 * (i.e. the "innovation covariance matrix"). This is a M*O x M*O matrix
	 * with M=length of "in_lm_indices_in_S".
	 * \param in_lm_indices_in_S The indices of the map landmarks (range
	 * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix
	 * in_S.
	 *
	 *  This method will be called just once for each complete KF iteration.
	 * \note It is assumed that the observations are independent, i.e. there
	 * are NO cross-covariances between them.
	 */
	virtual void OnGetObservationsAndDataAssociation(
		vector_KFArray_OBS& out_z, std::vector<int>& out_data_association,
		const vector_KFArray_OBS& in_all_predictions, const KFMatrix& in_S,
		const std::vector<size_t>& in_lm_indices_in_S,
		const KFMatrix_OxO& in_R) = 0;

	/** Implements the observation prediction \f$ h_i(x) \f$.
	 * \param idx_landmark_to_predict The indices of the landmarks in the map
	 * whose predictions are expected as output. For non SLAM-like problems,
	 * this input value is undefined and the application should just generate
	 * one observation for the given problem.
	 * \param out_predictions The predicted observations.
	 */
	virtual void OnObservationModel(
		const std::vector<size_t>& idx_landmarks_to_predict,
		vector_KFArray_OBS& out_predictions) const = 0;

	/** Implements the observation Jacobians \f$ \frac{\partial h_i}{\partial x}
	 * \f$ and (when applicable) \f$ \frac{\partial h_i}{\partial y_i} \f$.
	 * \param idx_landmark_to_predict The index of the landmark in the map
	 * whose prediction is expected as output. For non SLAM-like problems, this
	 * will be zero and the expected output is for the whole state vector.
	 * \param Hx  The output Jacobian \f$ \frac{\partial h_i}{\partial x} \f$.
	 * \param Hy  The output Jacobian \f$ \frac{\partial h_i}{\partial y_i}
	 * \f$.
	 */
	virtual void OnObservationJacobians(
		[[maybe_unused]] size_t idx_landmark_to_predict,
		[[maybe_unused]] KFMatrix_OxV& Hx,
		[[maybe_unused]] KFMatrix_OxF& Hy) const
	{
		m_user_didnt_implement_jacobian = true;
	}

	/** Only called if using a numeric approximation of the observation
	 * Jacobians, this method must return the increments in each dimension of
	 * the vehicle state vector while estimating the Jacobian.
	 */
	virtual void OnObservationJacobiansNumericGetIncrements(
		KFArray_VEH& out_veh_increments,
		KFArray_FEAT& out_feat_increments) const
	{
		for (size_t i = 0; i < VEH_SIZE; i++)
			out_veh_increments[i] = 1e-6;
		for (size_t i = 0; i < FEAT_SIZE; i++)
			out_feat_increments[i] = 1e-6;
	}

	/** Computes A=A-B, which may need to be re-implemented depending on the
	 * topology of the individual scalar components (eg, angles).
	 */
	virtual void OnSubstractObservationVectors(
		KFArray_OBS& A, const KFArray_OBS& B) const
	{
		A -= B;
	}

   public:
	/** 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 The observation vector whose inverse sensor model is to be
	 * computed. This is actually one of the vector<> returned by
	 * OnGetObservationsAndDataAssociation().
	 * \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: OBS_SIZE
	 *  - V: VEH_SIZE
	 *  - F: FEAT_SIZE
	 *
	 * \note OnNewLandmarkAddedToMap will be also called after calling this
	 * method if a landmark is actually being added to the map.
	 * \deprecated This version of the method is deprecated. The alternative
	 * method is preferred to allow a greater flexibility.
	 */
	virtual void OnInverseObservationModel(
		[[maybe_unused]] const KFArray_OBS& in_z,
		[[maybe_unused]] KFArray_FEAT& out_yn,
		[[maybe_unused]] KFMatrix_FxV& out_dyn_dxv,
		[[maybe_unused]] KFMatrix_FxO& out_dyn_dhn) const
	{
		MRPT_START
		THROW_EXCEPTION(
			"Inverse sensor model required but not implemented in derived "
			"class.");
		MRPT_END
	}

	/** If applicable to the given problem, this method implements the inverse
	 * observation model needed to extend the "map" with a new "element".
	 *  The uncertainty in the new map feature comes from two parts: one from
	 * the vehicle uncertainty (through the out_dyn_dxv Jacobian),
	 *   and another from the uncertainty in the observation itself. By
	 * default, out_use_dyn_dhn_jacobian=true on call, and if it's left at
	 * "true",
	 *   the base KalmanFilter class will compute the uncertainty of the
	 * landmark relative position from out_dyn_dhn.
	 *  Only in some problems (e.g. MonoSLAM), it'll be needed for the
	 * application to directly return the covariance matrix \a
	 * out_dyn_dhn_R_dyn_dhnT, which is the equivalent to:
	 *
	 *         \f$ \frac{\partial y_n}{\partial h_n} R \frac{\partial
	 * y_n}{\partial h_n}^\top \f$.
	 *
	 *  but may be computed from additional terms, or whatever needed by the
	 * user.
	 *
	 * \param in_z The observation vector whose inverse sensor model is to be
	 * computed. This is actually one of the vector<> returned by
	 * OnGetObservationsAndDataAssociation().
	 * \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$.
	 * \param out_dyn_dhn_R_dyn_dhnT See the discussion above.
	 *
	 *  - O: OBS_SIZE
	 *  - V: VEH_SIZE
	 *  - F: FEAT_SIZE
	 *
	 * \note OnNewLandmarkAddedToMap will be also called after calling this
	 * method if a landmark is actually being added to the map.
	 */
	virtual void OnInverseObservationModel(
		const KFArray_OBS& in_z, KFArray_FEAT& out_yn,
		KFMatrix_FxV& out_dyn_dxv, KFMatrix_FxO& out_dyn_dhn,
		[[maybe_unused]] KFMatrix_FxF& out_dyn_dhn_R_dyn_dhnT,
		bool& out_use_dyn_dhn_jacobian) const
	{
		MRPT_START
		OnInverseObservationModel(in_z, out_yn, out_dyn_dxv, out_dyn_dhn);
		out_use_dyn_dhn_jacobian = true;
		MRPT_END
	}

	/** If applicable to the given problem, do here any special handling of
	 * adding a new landmark to the map.
	 * \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.
	 * \sa OnInverseObservationModel
	 */
	virtual void OnNewLandmarkAddedToMap(
		[[maybe_unused]] size_t in_obsIdx,
		[[maybe_unused]] size_t in_idxNewFeat)
	{
		// Do nothing in this base class.
	}

	/** 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()
	{
		// Do nothing in this base class.
	}

	/** This method is called after finishing one KF iteration and before
	 * returning from runOneKalmanIteration().
	 */
	virtual void OnPostIteration()
	{
		// Do nothing in this base class.
	}

	/** @}
	 */

   public:
	CKalmanFilterCapable()
		: mrpt::system::COutputLogger("CKalmanFilterCapable"),
		  KF_options(this->m_min_verbosity_level)
	{
	}
	/** Destructor */
	~CKalmanFilterCapable() override = default;
	mrpt::system::CTimeLogger& getProfiler() { return m_timLogger; }
	/** Generic options for the Kalman Filter algorithm itself. */
	TKF_options KF_options;

   private:
	//  "Local" variables to runOneKalmanIteration, declared here to avoid
	//   allocating them over and over again with each call.
	//  (The variables that go into the stack remains in the function body)
	vector_KFArray_OBS m_all_predictions;
	std::vector<size_t> m_predictLMidxs;
	/** The vector of all partial Jacobians dh[i]_dx for each prediction */
	std::vector<KFMatrix_OxV> m_Hxs;
	/** The vector of all partial Jacobians dh[i]_dy[i] for each prediction */
	std::vector<KFMatrix_OxF> m_Hys;
	KFMatrix m_S;
	vector_KFArray_OBS m_Z;	 // Each entry is one observation:
	KFMatrix m_K;  // Kalman gain
	KFMatrix m_S_1;	 // Inverse of m_S
	KFMatrix m_dh_dx_full_obs;
	KFMatrix m_aux_K_dh_dx;

   protected:
	/** 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 https://www.mrpt.org/Kalman_Filters
	 */
	void runOneKalmanIteration();

   private:
	mutable bool m_user_didnt_implement_jacobian{true};

	/** Auxiliary functions for Jacobian numeric estimation */
	static void KF_aux_estimate_trans_jacobian(
		const KFArray_VEH& x, const std::pair<KFCLASS*, KFArray_ACT>& dat,
		KFArray_VEH& out_x);
	static void KF_aux_estimate_obs_Hx_jacobian(
		const KFArray_VEH& x, const std::pair<KFCLASS*, size_t>& dat,
		KFArray_OBS& out_x);
	static void KF_aux_estimate_obs_Hy_jacobian(
		const KFArray_FEAT& x, const std::pair<KFCLASS*, size_t>& dat,
		KFArray_OBS& out_x);

	template <
		size_t VEH_SIZEb, size_t OBS_SIZEb, size_t FEAT_SIZEb, size_t ACT_SIZEb,
		typename KFTYPEb>
	friend void detail::addNewLandmarks(
		CKalmanFilterCapable<
			VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>& obj,
		const typename CKalmanFilterCapable<
			VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb,
			KFTYPEb>::vector_KFArray_OBS& Z,
		const std::vector<int>& data_association,
		const typename CKalmanFilterCapable<
			VEH_SIZEb, OBS_SIZEb, FEAT_SIZEb, ACT_SIZEb, KFTYPEb>::KFMatrix_OxO&
			R);
};	// end class

}  // namespace bayes
}  // namespace mrpt

MRPT_ENUM_TYPE_BEGIN(mrpt::bayes::TKFMethod)
using namespace mrpt::bayes;
MRPT_FILL_ENUM(kfEKFNaive);
MRPT_FILL_ENUM(kfEKFAlaDavison);
MRPT_FILL_ENUM(kfIKFFull);
MRPT_FILL_ENUM(kfIKF);
MRPT_ENUM_TYPE_END()

// Template implementation:
#define CKalmanFilterCapable_H
#include "CKalmanFilterCapable_impl.h"
