/*---------------------------------------------------------------
	FILE: CMonoSlamInterface.h

	Clase CMonoSlamInterface

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

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

#include <MRPT/MRML/CMonoSlam.h>
#include <MRPT/UTILS/CFeature.h>
#include <MRPT/UTILS/EKF.h>
#include <MRPT/UTILS/IEKF.h>
#include <MRPT/UTILS/UKF.h>
#include <MRPT/UTILS/HybridKF.h>
#include <MRPT/UTILS/CConfigFile.h>
#include <MRPT/UTILS/CMRPTImage.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CDisplayWindow.h>
#include <MRPT/UTILS/CDisplayWindow3D.h>
#include <MRPT/MRVL/CStereoGrabber_Bumblebee.h>
#include <MRPT/MRML/CObservationStereoImages.h>
#include <MRPT/MRVL/CImageGrabber_OpenCV.h>
#include <MRPT/MRML/CActionObservationSequence.h>
#include <MRPT/MRML/CSensorialFrame.h>

namespace MRML
{
	class CMonoSlamInterface
	{
	/** This class provide a user interface to use easily the MonoSLAM
	 */
	protected:
		/*Members*/
		CMonoSlam						ms;
		UTILS::CFeature					feats;
		UTILS::EKF						kalman;
		UTILS::UKF						ukalman;
		UTILS::IEKF						ikalman;
		UTILS::HybridKF					hkalman;
		int								sim_or_rob;		//if this value is '0' we use snapshot, in other case we use a camera in real time
		float							prev_speed;		//speed of previous iteration (for avoid strong movements)
		UTILS::CVectorTemplate<float>	prev_xv;		//state of previous iteration
		float							dt;				//dt --> delta_t (for timestamp in capture mode)
		int								store_index;	//indice para almacenar los datos de la camara acorde con el GPS
		UTILS::CDisplayWindow			*wind2D;
		UTILS::CDisplayWindow3D			*wind3D;
		//UTILS::COpenGLScene				**world;		// JLBC 2007: Use the critical sections!!
		MRVL::CImageGrabber_OpenCV		*camera;
		MRVL::CStereoGrabber_Bumblebee	*bublebee_cam;
		float							tiempo_match;
		int								pasadas;

		/** Show results (features selected and tracked) over actual frame.
		*/
		void  plot_results_2D(UTILS::CMatrixTemplateNumeric<float>					&zz,
										UTILS::CMatrixTemplateNumeric<float>					&hh,
										UTILS::CMatrixTemplateNumeric<float>					&h,
										UTILS::CMatrixTemplateNumeric<float>					&S,
										UTILS::CMatrixTemplateNumeric<float>					&XYZ,
										UTILS::CMRPTImage				&framecolor,
										unsigned int					type,
										UTILS::CVectorTemplate<float>	&match,
										bool							verbose,
										bool							savedatas);

		/** Show the camera and feature estimated position in a 3D world
		*/

		void  plot_results_3D(UTILS::CMatrixTemplateNumeric<float> &XYZ);

		/** This function is only a short method for plot in 3D windows
		*/

		/*
		void  plot3D()
		{
			if (!wind3D) wind3D = new UTILS::CDisplayWindow3D("3D WORLD MONOSLAM",960,590);
			wind3D->unlockAccess3DScene();
			world = wind3D->get3DSceneAndLock();
			wind3D->unlockAccess3DScene();
			wind3D->forceRepaint();
		}
		*/
    public:
		/** Constructor
		 */
		CMonoSlamInterface(const std::string &file);


		/** Destructor
		  */
		~CMonoSlamInterface();

		/**	This function initialize features from first frame.
		 */
		void  initialize(CSensorialFrame *sensorialframe,
									bool show2D=true,
									bool show3D=true,
									bool verbose=true,
									bool savedatas=true);

		/** This function do a full step in MonoSlam Process.
		*	- Load Image (from file or from a camera, it depend of sim_or_rob flag)
		*	- type determine the kalman filter to use:
		*			- 0:  EKF  (Extended Kalman Filter)(default)
		*			- 1:  IEKF (Iterative Extended Kalman Filter)
		*			- 2:  UKF  (Unscented Kalman Filter)
		*	- Search Features if we have less than necessaries
		*	- EKF Prediction Step
		*	- Matching/Tracking of Features
		*	- EKF Update Step
		*	- Show Results
		*/
		void  go_one_step(CSensorialFrame *sensorialframe,
									unsigned int type=0,
									bool show2D=true,
									bool show3D=true,
									bool verbose=true,
									bool savedatas=true);

	}; // end class

} // end namespace

#endif //__CCMonoSlam_H*/
