/*---------------------------------------------------------------
	FILE: CMonoSlam.h

	Clase CMonoSlam

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

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

#include <math.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <MRPT/UTILS/CQuaternion.h>
#include <MRPT/UTILS/CMonoSlamMotionModel.h>
#include <MRPT/UTILS/CConfigFile.h>
#include <MRPT/UTILS/CMRPTImage.h>
#include <MRPT/UTILS/CFeature.h>
#include <MRPT/UTILS/KFCapable.h>
#include <MRPT/MRVL/CCamModel.h>
#include <MRPT/MRVL/CFeatureExtraction.h>

namespace MRML
{
	class CMonoSlam:public UTILS::KFCapable
	{
		/**	Master class which defines and runs the MonoSLAM real-time single
		 * camera SLAM application. This class is GUI-independent, and only provides the
		 * core interface, for clarity and simplicity. The class MonoSLAMInterface
		 * derives from this and should usually be used instead, as it provides the rest
		 * of the interface.
		 */
	protected:

		UTILS::CMonoSlamMotionModel	mmm;					/**MonoSLAMMotionModel*/
		MRVL::CCamModel				cam;					/**Camera model*/
		unsigned int				state_size;				/**State Vector Size*/
		unsigned int				feature_size;			/**Feature Vector Size*/
		double						match_threshold_up;		/**Top threshold for feature classification*/
		double						match_threshold_down;	/**Botton threshold for feature classification*/
		float						threshold_ini;			/**Minimum quality for a feature in the initialization*/
		float						threshold_new;			/**Minimum quality for a feature in the searching of a new mark*/
		int							radius;					/**Radius of the image free area around to feature*/
		float						sigma;					/**Sigma value in the Harris method*/


		/** Calculate the Jacobian dh_dxv
		 */
		void  dh_dxv(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dh_dxv);

		/** Calculate the Jacobian dh_dy
		 */
		void  dh_dy(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dh_dy);

		/** Calculate the Jacobian dh_drw
		 */
		void  dh_drw(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dh_drw);

		/** Calculate the Jacobian dh_dqwr
		 */
		void  dh_dqwr(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dh_dqwr);

		/** Calculate the Jacobian dh_dhrl
		 */
		void  dh_dhrl(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dh_dhrl);

		/** Calculate the Jacobian dhrl_drw
		 */
		void  dhrl_drw(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &m_dhrl_drw);

		/** Calculate the Jacobian dhd_dhu
		 */
		void  dhd_dhu(UTILS::CMatrixTemplateNumeric<float> &zi, UTILS::CMatrixTemplateNumeric<float> &m_dhd_dhu);

		/** Calculate the Jacobian dhu_dhrl
		 */
		void  dhu_dhrl(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &m_dhu_dhrl);

		/** Calculate the Jacobian dhrl_dqwr
		 */
		void  dhrl_dqwr(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &m_dhrl_dqwr);

		/** Calculate the Jacobian dhrl_dy
		 */
		void  dhrl_dy(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &m_dhrl_dy);


		/** Calculate and return the feature coordinate in Unified Inverse Depth Parametrization. Where h_LR_rot
		 * is the vector which join the camera optic center to the feature.
		 */
		UTILS::CVectorTemplate<float>  hinv(UTILS::CMatrixTemplateNumeric<float> h_LR_rot);

		/** Calculate and return the vector which join the camera optic center to the feature.
		 */
		UTILS::CMatrixTemplateNumeric<float> calculate_ray(const float &col,const float &row);

		/** This function increment the covariance matrix with a new feature.
		*/
		void  addAFeatureCov_newPar(UTILS::CMatrixTemplateNumeric<float> &XYZ_w, float &col, float &row);

		/** Calculate the mahalanobis distance in feature innovation (prediction - measurement)
		 */
		void  mahalanob_dist(int j_k, int i_k, int halfW_x, int halfW_y, CMatrixTemplateNumeric<float> &inv_S, CMatrixTemplateNumeric<float> &correlationScore);

    public:


		/** Constructor
		*/
		CMonoSlam(const std::string &file);

		/** This function insert a new feature to the map (state vector and covariance matrix).
		 * \param col is the feature column position in pixels over the image
		 * \param row is the feature row position in pixels over the image
		 */
		void  addNewFeatures_newPar(float &col,float &row);

		/** Calculate the feature's position and the transition matrix H = [dh_dxv  dh_dy]
		 *	Measurement, derivatives and texture prediction
		 * \param h, output. This vector contain the feature position estimation.
		 * \param H, output. This matrix is the Jacovian of measurement prediction.
		 */
		void  h_calculate(UTILS::CMatrixTemplateNumeric<float> &h, UTILS::CMatrixTemplateNumeric<float> &H);

		/** This function return the position,zi (in pixels) of a feature yi
		 */
		void  hi(UTILS::CVectorTemplate<float> &yinit, UTILS::CMatrixTemplateNumeric<float> &zi );


		/** % Calculate the projection of yi (in the camera reference)
 		 */
		void  hu(UTILS::CMatrixTemplateNumeric<float> &hrl, UTILS::CVectorTemplate<float> &uv_u );


		/** % Calculate h derivates (h = feature's position in pixels).	Hi = [dh_dxv dh_dyi]
		 * \param yi --> feature in Unified Inverse Depth Parametrization
		 * \param zi --> measurement in pixels coordinates
		 * \param i  --> feature index
		 * \param Hi --> measurement jacobian
		 */
		void  calculate_Hi(UTILS::CVectorTemplate<float> &yi, UTILS::CMatrixTemplateNumeric<float> &zi, unsigned int i, UTILS::CMatrixTemplateNumeric<float> &Hi);

		/** Matching between actual image and feature patch	(FOR EKF)
		 \code
		 	input parameters
		 	h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn))
		 	S_predicted --> predicted covariance innovation matrix
		 	H_predicted --> predicted transition matrix
		 	im			--> current frame

		 	output parameters
		 	h			--> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 	z			--> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 	H_matched	--> measured transition matrix
		 	R_matched	--> measured process noise matrix

		 	inout parameter
		 	features	--> (As input) features struct to get feature's patch
		 					(As output)update hits for erase bad features later
		 \endcode
		 */
		void  MSMatching(UTILS::CMatrixTemplateNumeric<float> &h_predicted,  UTILS::CMatrixTemplateNumeric<float> &S_predicted,
								   UTILS::CMatrixTemplateNumeric<float> &H_predicted,  UTILS::CMRPTImage &im,
								   UTILS::CMatrixTemplateNumeric<float> &h,			  UTILS::CMatrixTemplateNumeric<float> &z,
								   UTILS::CMatrixTemplateNumeric<float> &H_matched,	  UTILS::CMatrixTemplateNumeric<float> &R_matched,
								   UTILS::CFeature &features);

		/** Matching between actual image and feature patch	(FOR UKF and IEKF)
		\code
			input parameters
			h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn))
			S_predicted --> predicted covariance innovation matrix
			im			--> current frame

			output parameters
			z			--> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
			R_matched	--> measured process noise matrix

			inout parameter
			features	--> (As input) features struct to get feature's patch
							(As output)update hits for erase bad features later
		\endcode
		*/
		void  MSMatching(UTILS::CMatrixTemplateNumeric<float> &h_predicted,				UTILS::CMatrixTemplateNumeric<float> &S_predicted,
									  UTILS::CMRPTImage &im,					UTILS::CMatrixTemplateNumeric<float> &h,
									  UTILS::CMatrixTemplateNumeric<float> &z,						UTILS::CMatrixTemplateNumeric<float> &R_matched,
									  UTILS::CVectorTemplate<float> &match,	UTILS::CFeature &features);


		/** Initialize feature employing the Harris method adding neccesary information to x(k|k) and p(k|k)
		 * \param features	--> Structure to store the visual features
		 * \param im		--> Initial image in the system
		 */
		void  InitializeNewFeature(UTILS::CFeature &features, UTILS::CMRPTImage &im);

		/** Try to find a image area without features and search it employing the Harris method adding neccesary information to x(k|k) and p(k|k)
		 * \param features	--> Structure to store the visual features
		 * \param im		--> Image where search more features
		 * \param h			--> Predicted features over the image
		 */
		void  SearchMoreFeature(UTILS::CFeature &features, UTILS::CMRPTImage &im, UTILS::CMatrixTemplateNumeric<float> &h);


		/** This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the EKF.
		 * \param HH		--> Measured transition matrix
		 * \param RR		--> Measured process noise matrix
		 * \param hh		--> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 * \param zz		--> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 */
		void  Ambiguity(CMatrixTemplateNumeric<float> & HH,CMatrixTemplateNumeric<float> &RR,CMatrixTemplateNumeric<float> &hh,CMatrixTemplateNumeric<float> &zz);

		/** This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the IEKF or UKF.
		 * \param RR		--> Measured process noise matrix
		 * \param hh		--> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 * \param zz		--> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
		 * \param match		--> This vector contain the information about which features has been correctly measured and which not.
		 */
		void  Ambiguity(CMatrixTemplateNumeric<float> & RR,CMatrixTemplateNumeric<float> &hh,CMatrixTemplateNumeric<float> &zz,CVectorTemplate<float> &match);

/********************************************   FUNCTIONS FOR EKF   ********************************************/
		/** Generic function to acces to the prediction model: fv (it must be implemented in MonoSLAMMotionModel)
		 *	\param u --> odometry information (unused parameter for MonoSLAM)
		 *	\param xv --> camera state
		 */
		void  function_fv(UTILS::CMatrixTemplateNumeric<float> &u, UTILS::CVectorTemplate<float> &xv);

		/** Generic function to acces to the prediction model Jacobian, F due to the linealization process
		 * (it must be implemented in MonoSLAMMotionModel)
		 */
		void  function_F(UTILS::CMatrixTemplateNumeric<float> &F_parcial);

		/** Generic function to acces to the matrix process noise, Q
		 * (it must be implemented in MonoSLAMMotionModel)
		 */
		void  function_Q(UTILS::CMatrixTemplateNumeric<float> &Q);

		/** Measurement model
		 * \param h --> Vector to return the measurement prediction coordinates
		 * \param Hx --> Jacobian Hx
		 * \param Hx --> Jacobian Hz (unused for monoSLAM)
		 */
		void  function_h(UTILS::CMatrixTemplateNumeric<float> &h, UTILS::CMatrixTemplateNumeric<float> &Hx, UTILS::CMatrixTemplateNumeric<float> &Hz);

		/** Generic function to acces to the measurement process noise, R
		 * (it must be implemented in MonoSLAMMotionModel)
		 */
		void  function_R(UTILS::CMatrixTemplateNumeric<float> &h, UTILS::CMatrixTemplateNumeric<float> &R);

		/** This function normalize the state vector and covariance matrix due to the quaternion employ for orientation.
		 * If the orientation is not represented by a quaternion, this function is empty.
		 */
		void  normalize();

		/** This function delete from the state vector and covariance matrix the feature in the position i.
		 */
		void delete_feature(const unsigned int &index);

		/** Return the state size. In Monocular SLAM this value is 13
		 */
		unsigned int  get_state_size(){return state_size;}

		/** Return the feature size. In Monocular SLAM this value is 6 due to the employ of Unified Inverse Depth Parametrization.
		 */
		unsigned int  get_feature_size(){return feature_size;}

		/** Transform the Unified Inverse Depth Parametrization coordinates to XYZ cartesian coordinates.
		 */
		UTILS::CMatrixTemplateNumeric<float>  XYZ6D_TO_XYZ3D(){return mmm.XYZTPL2XYZ(xkk);}

		/** Transform the Unified Inverse Depth Parametrization covariance matrix (6x6) to XYZ covariance (3x3).
		 */
		UTILS::CMatrixTemplateNumeric<float>  pxyztpl2pxyz(){return mmm.pxyztpl2pxyz(pkk, xkk);}


/********************************************   FUNCTIONS FOR uKF   ********************************************/
		/** Measurement model
		 * \param h --> Vector to return the measurement prediction coordinates
		 */
		void  h_UKF(UTILS::CMatrixTemplateNumeric<float> &h);

		/** Visibility test for determine which features are shown in the image
		 * \param h --> Vector to return the measurement prediction coordinates
		 */
		void  visibility(UTILS::CMatrixTemplateNumeric<float> &h);


/************************************************   NEWBITS   ***************************************************/
		/** Jacobian of quaternion inverse: this is a constant matrix that doesn't
		* depend on the quaternion
		*/
        CMatrixTemplateNumeric<float>  dqbar_by_dq();	//matrix(4x4)

		/** Component Jacobians: each returns a 3x3 matrix which is the derivative
		* of the rotation matrix derived from a quaternion w.r.t. return the real element
		*/
		UTILS::CMatrixTemplateNumeric<float>  dR_by_dq0(UTILS::MATH::CQuaternion<float> &q);	//matrix(3x3)

		/** Component Jacobians: each returns a 3x3 matrix which is the derivative
		* of the rotation matrix derived from a quaternion w.r.t. the imaginary element x
		*/
        UTILS::CMatrixTemplateNumeric<float>  dR_by_dqx(UTILS::MATH::CQuaternion<float> &q);	//matrix(3x3)

		/** Component Jacobians: each returns a 3x3 matrix which is the derivative
		* of the rotation matrix derived from a quaternion w.r.t. the imaginary element y
		*/
        UTILS::CMatrixTemplateNumeric<float>  dR_by_dqy(UTILS::MATH::CQuaternion<float> &q);	//matrix(3x3)

		/** Component Jacobians: each returns a 3x3 matrix which is the derivative
		* of the rotation matrix derived from a quaternion w.r.t. the imaginary element z
		*/
        UTILS::CMatrixTemplateNumeric<float>  dR_by_dqz(UTILS::MATH::CQuaternion<float> &q);	//matrix(3x3)


		/** Auxiliary functions used by dqnorm_by_dq(). Value of diagonal element of Jacobian
		*/
        float  dqi_by_dqi(float qi, float qq);

		/** Value of off-diagonal element of Jacobian
		*/
        float  dqi_by_dqj(float qi, float qj, float qq);


		/** Normalising Jacobian
		*/
        UTILS::CMatrixTemplateNumeric<float>  dqnorm_by_dq(UTILS::MATH::CQuaternion<float> &q);	//matrix (4x4)

		/** Normalising a 3D vector
		 */
        UTILS::CMatrixTemplateNumeric<float>  dvnorm_by_dv(UTILS::CVectorTemplate<float> &v);	//matrix(3x3)

		/** Auxiliary function. User must not be used.
		 * \param q --> state vector quaternion.
		 * \param a --> (hLhatRi) Not rotate.
		 */
        UTILS::CMatrixTemplateNumeric<float>  dRq_times_a_by_dq (UTILS::MATH::CQuaternion<float> &q, UTILS::CMatrixTemplateNumeric<float> &aMat); //matrix(3x4)

/***********************************************************************/
//ESTA FUNCION SOLO SIRVE PARA PROBAR LA CLASE DESDE MATLAB, BORRAR CND TERMINE LA DEPURACION

		/**	This function set the state vector value. (Employed for debug the program)
		 */
		void  set_xkk(const UTILS::CVectorTemplate<float> &xkk_in){xkk = xkk_in;}

		/**	This function set the covariance matrix value. (Employed for debug the program)
		 */
		void  set_pkk(const UTILS::CMatrixTemplateNumeric<float> &pkk_in){pkk = pkk_in;}

		/**	This function read the state vector value. (Employed for debug the program)
		 */
		void  get_xkk(UTILS::CVectorTemplate<float> &xkk_out)
		{
			xkk_out.resize(xkk.size());
			xkk_out = xkk;
		}

		/**	This function read the covariance matrix value. (Employed for debug the program)
		 */
		void  get_pkk(UTILS::CMatrixTemplateNumeric<float> &pkk_out){pkk_out = pkk;}

	}; // end class

} // end namespace

#endif //__CCMonoSlam_H
