/*---------------------------------------------------------------
	FILE: KFCapable.h

	Clase KFCapable

   ISA - Universidad de Malaga - http://www.isa.uma.es

   Autor: Antonio J. Ortiz de Galisteo (AJOGD)
  ---------------------------------------------------------------*/
#ifndef KFCapable_H
#define KFCapable_H

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

namespace UTILS
{
	class KFCapable
	{
		/** This class provide a unified interface to the Kalman Filters: EKF, IEKF and UKF.
		 */
		friend class EKF;
		friend class IEKF;
		friend class UKF;
		friend class HybridKF;
//		friend class SRUKF;

    protected:

		CVectorTemplate<float> xkk;
		CMatrixTemplateNumeric<float> pkk;

	public:
        /** Virtual destructor required for virtual classes.
		 */
		virtual ~KFCapable()
		{
		}

		/** This virtual function musts implement the prediction model of the Kalman filter.
		 */
		virtual	void  function_fv(CMatrixTemplateNumeric<float> &u, CVectorTemplate<float> &xv)=0;

		/** This virtual function musts calculate the Jacobian F of the prediction model.
		 */
		virtual void  function_F(CMatrixTemplateNumeric<float> &F_parcial)=0;

		/** This virtual function musts calculate de noise matrix of the prediction model.
		 */
		virtual void  function_Q(CMatrixTemplateNumeric<float> &Q)=0;

		/** This virtual function musts implement the observation model of the Kalman filter (EKF or IEKF only).
		 */
		virtual void  function_h(CMatrixTemplateNumeric<float> &h, CMatrixTemplateNumeric<float> &Hx, CMatrixTemplateNumeric<float> &Hz)=0;

		/** This virtual function musts calculate the noise measurement matrix of the Kalman filter.
		 */
		virtual void  function_R(CMatrixTemplateNumeric<float> &h, CMatrixTemplateNumeric<float> &R)=0;

		/** This virtual function musts normalize the state vector and covariance matrix (only if its necessary).
		 */
		virtual void  normalize()=0;

		/** This virtual function musts return the dimension of the state.
		 */
		virtual unsigned int  get_state_size()=0;

		/** This virtual function musts return the dimension of the feature.
		 */
		virtual unsigned int  get_feature_size()=0;

		/** This virtual function musts return the size of the state vector.
		 */
		virtual unsigned int  get_xkk_size(){return (unsigned int)xkk.size();}

		/** This function is only employ in MonoSLAM to read the camera state.
		 */
		virtual void  get_xv(CVectorTemplate<float> &xv_out)
		{
			xv_out.resize(get_state_size());
			for (size_t i=0;i<get_state_size();i++)
				xv_out[i]=xkk[i];
		}

		/** This function is only employ in MonoSLAM to write the camera state.
		 */
		virtual void  set_xv(const CVectorTemplate<float> &xv_new)
		{
			if (xv_new.size()!=get_state_size())
				THROW_EXCEPTION("KFCapable::set_xv: Wrong size of input parameter");
            for (size_t i=0;i<get_state_size();i++)
				xkk[i]=xv_new[i];
		}

		/** This function is only employ in MonoSLAM to read the camera covariance.
		 */
		virtual CMatrixTemplateNumeric<float>  get_pxx(){return pkk.extractMatrix(0,0,get_state_size(),get_state_size());}

		/** This function is only employ in MonoSLAM to write the camera covariance.
		 */
		virtual void  set_pxx(const CMatrixTemplateNumeric<float> &pxx_new)
		{
			if ((pxx_new.getRowCount() != get_state_size()) || (pxx_new.getColCount() != get_state_size()))
				THROW_EXCEPTION("Wrong Pxx size in KFCapable::set_pxx");
			pkk.insertMatrix(0,0,pxx_new);
		}

		/** This virtual function musts implement the observation model of the Unscented Kalman filter.
		 */
		virtual void  h_UKF(UTILS::CMatrixTemplateNumeric<float> &h)=0;

		/** This function is only employ in MonoSLAM to apply a visibility test to the measurements in UKF update.
		 */
		virtual void  visibility(UTILS::CMatrixTemplateNumeric<float> &h)=0;
	}; // end class

} // end namespace

#endif //__KFCapable_H
