/*---------------------------------------------------------------
	FILE: CMonoSlamMotionModel.h

	Clase CMonoSlamMotionModel

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

   Autor: Antonio J. Ortiz de Galisteo (AJOGD)
  ---------------------------------------------------------------*/

#ifndef CMonoSlamMotionModel_H
#define CMonoSlamMotionModel_H

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <MRPT/UTILS/CQuaternion.h>

namespace UTILS
{
	/** General motion model in 3D
	* Assumes a random impulse changes the velocity at each time step
	*	Impulse ThreeD Motion Model
	* Class to represent a constant-velocity motion model.
	* A constant-velocity motion model does not mean that the camera moves at a constant velocity over all time,
	* but that the statistical model of its motion in a timestep is that on average we expect undetermined accelerations
	* occur with a Gaussian profile. At each timestep there is a random impulse, but these are normally-distributed.
	* State vector: 13 elements
	\code
	                 x
	  r              y
	                 z
	  -              -
	                 q0
	  q              qx
	                 qy
	                 qz
	  -              -
	                 vx
	  v              vy
	                 vz
	  -              -
	                 omegax
	  omega          omegay
	                 omegaz
	\endcode
	* Control vector has 3 elements, and represents a measurement of
	* acceleration if a linear accelerometer is present. Otherwise set it
	* to zero.
	\code
	 Noise vector n = V
	                  Omega
    \endcode
	* Update:
	\code
	 rnew     =   r + (v + V) delta_t
	 qnew     =   q x q((omega + Omega) delta_t)
	 vnew     =   v + V
	 omeganew =   omega + Omega
	\endcode
	* Here we can use de specific dimension because is a specific class for monocular SLAM with
	* Inverse Depth Parametrization
	*/
	class CMonoSlamMotionModel
	{
	protected:

		float					lambda_init;		//Initial Inverse Depth estimation
		float					std_lambda;			//Inverse Depth Variance
		unsigned int			state_size;			//State Vector Size "xv" (in monoslam will be 13)
		unsigned int			feature_size;		//Feature size stored in xkk (in monoslam will be 6)
		std::string				path;				//images path
		unsigned int			initial_image;		//first image index
		unsigned int			num_image;			//numer of total images
		unsigned int			step;				//iteration
		CMatrixTemplateNumeric<float>				chi2inv;			//inverse values table for chi^2
		float					sigma_image_noise;	//image noise in pixels
		float					sigma_a_noise;		//linear velocity noise in m/s^2
		float					sigma_alpha_noise;	//angular velocity noise in rad/s^2
		float					delta_t;			//time between frames


	/** Calculate commonly used Jacobian part dq(omega * delta_t) by domega
	*/
		void dqomegadt_by_domega(const CVectorTemplate<float> &omega, CMatrixTemplateNumeric<float> &RESdqomegadt_by_domega);


	/** Ancillary function to calculate part of Jacobian partial q / partial omega which is repeatable
	*	due to symmetry. Here omegaA is one of omegax, omegay, omegaz.
	*/
		float dq0_by_domegaA(float omegaA, float omega);

	/** Ancillary function to calculate part of Jacobian which is repeatable due
	* to symmetry. Here omegaA is one of omegax, omegay, omegaz and similarly with qA.
	*/
		float dqA_by_domegaA(float omegaA, float omega);

	/** Ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
	* Here omegaB is one of omegax, omegay, omegaz and similarly with qA.
	*/
		float dqA_by_domegaB(float omegaA, float omegaB,float omega);

	/** Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
	*/
		void dq3_dq1(MATH::CQuaternion<float>& q2,CMatrixTemplateNumeric<float> &M);

	/** Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
	*/
		void dq3_dq2(MATH::CQuaternion<float>& q1,CMatrixTemplateNumeric<float> &M);

	public:
		/** Default constructor
		  */
		CMonoSlamMotionModel();

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

		/** Return the initial detph estimation
		 */
		float  get_lambda_init()const {return lambda_init;}

		/** Return the variance of initial detph estimation
		 */
		float  get_std_lambda()const {return std_lambda;}

		/** Return the dimension of a feature (in monoSLAM employing Unified Inverse Depth Parametrization is 6)
		 */
		unsigned int  get_feature_size()const {return feature_size;}

		/** Return the dimension of a feature (in monoSLAM employing Unified Inverse Depth Parametrization is 6)
		 */
		void  get_feature_size(unsigned int &s)const {s = feature_size;}

		/** Return the state dimension (in monoSLAM employing Unified Inverse Depth Parametrization is 6)
		 */
		unsigned int  get_state_size()const {return state_size;}

		/** Return the state dimension (in monoSLAM employing Unified Inverse Depth Parametrization is 6)
		 */
		void  get_state_size(unsigned int &s)const {s = state_size;}

		/** Return the value of inverse chi function in row 'r' and column 'c'.
		 */
		float  get_chi2inv(const unsigned int &r,const unsigned int &c)const {return chi2inv(r,c);}

		/** Return all values of inverse chi function stored in the matrix.
		 */
		CMatrixTemplateNumeric<float>  get_chi2inv()const {return chi2inv;}

		/** Return the path where data are stored
		 */
		std::string  get_path()const {return path;}

		/** Return the path where data are stored
		 */
		void  get_path(std::string &s)const {s = path;}

		/** Return the frame index in simulation mode
		 */
		unsigned int  get_step()const {return step;}

		/** Return the frame index in simulation mode
		 */
		void  get_step(unsigned int &f)const {f = step;}

		/** Set the frame index to begin the simulation
		 */
		void  set_step(const unsigned int &s) {step = s;}

		/** Set the time between frames
		 */
		void  set_delta_t(const float &dt) {delta_t = dt;}

		/** Read the variance of image measurement method
		 */
		float  get_sigma_image_noise()const {return sigma_image_noise;}

		/** Read the variance of lineal velocity
		 */
		float  get_sigma_a_noise()const {return sigma_a_noise;}

		/** Read the variance of angular velocity
		 */
		float  get_sigma_alpha_noise()const {return sigma_alpha_noise;}

		/** Read time between frames
		 */
		float  get_delta_t()const {return delta_t;}

	/** Extract from state vector their components:
	 *	\param xv --> State Vector
	 *	\param r --> camera position
	 *	\param q --> quaternion
	 *	\param v --> linear velocity
	 *	\param w --> angular velocity
	 */
		void extract_components(CVectorTemplate<float> &xv,
							CVectorTemplate<float> &r,
							MATH::CQuaternion<float> &q,
							CVectorTemplate<float> &v,
							CVectorTemplate<float> &w);

	/** Form the state vector from their individual components
	 *	\param xv --> State Vector
	 *	\param r --> camera position
	 *	\param q --> quaternion
	 *	\param v --> linear velocity
	 *	\param w --> angular velocity
	 */
		void compose_xv(CVectorTemplate<float> &xv,
							const CVectorTemplate<float> &r,
							const MATH::CQuaternion<float> &q,
							const CVectorTemplate<float> &v,
							const CVectorTemplate<float> &w);

	/** This function predict the state vector: Prediction model (camera position, angle, linear velocity and angular velocity)
	*	\param xv is the actual state vector
	*	\param r_{new} = r + (v+V)*Delta t
	*   \param q_{new} = q * q(omega + Omega) * Delta t)
	*   \param v_{new} = v + V
	*   \param w_{new} = w + W
	*	[x y z qr qx qy qz vx vy vz wx wy wz]
	*/
		void func_fv(CVectorTemplate<float> &xv);

	/** Calculate the Jacobian 'dfv/dxv'
	\code
		Identity is a good place to start since overall structure is like this
		I       0             I * delta_t   0
		0       dqnew_by_dq   0             dqnew_by_domega
		0       0             I             0
		0       0             0             I
	\endcode
	*/
		void func_dfv_by_dxv(CVectorTemplate<float> &xv, CMatrixTemplateNumeric<float> &F);

	/** Calculate the covariance noise matrix associated to the process 'Q'
	*	It's necessary to calculate the prediction 'P(k+1|k)'
	*   Fill noise covariance matrix Pnn: this is the covariance of  the noise vector [V Omega]^T
	*	that gets added to the state.
	*	Form of this could change later, but for now assume that
	*	V and Omega are independent, and that each of their components is independent.
	*/
		void func_Q(CVectorTemplate<float> &xv,CMatrixTemplateNumeric<float> &QRES);

	/** In the case of Monocular SLAM this Jacobian is a 7x7 identity matrix
	*
	*/
		void func_dxp_by_dxv(const CVectorTemplate<float> &xv,CMatrixTemplateNumeric<float> &m);


	/** Convert coordenates from inverse detph parametrization (xc,yc,zc,theta,phi,lambda) to 3D (x,y,z) coordinates
	*	Return a matrix where each row is a feature whit:
	*	- column 0 --> X coordinate
	*	- column 1 --> Y coordinate
	*	- column 2 --> Z coordinate
	*/
		CMatrixTemplateNumeric<float>  XYZTPL2XYZ(CVectorTemplate<float> &xkk);

	/** Convert covariance matrix from inverse detph parametrization diag(xc,yc,zc,theta,phi,lambda) to 3D
	*	covariance matrix diag(x,y,z).
	*/
		CMatrixTemplateNumeric<float>  pxyztpl2pxyz(CMatrixTemplateNumeric<float> &pkk, CVectorTemplate<float> &xkk);
 	}; // end class

} // end namespace

#endif //__CMonoSlamMotionModel.h
