/*---------------------------------------------------------------
	FILE: CStereoServerBumblebee.h
	USE: See doxygen doc.

	Part of the MRPT Library
	ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef CStereoServerBumblebee_H
#define CStereoServerBumblebee_H

#include <MRPT/MRML/CObservationVisualLandmarks.h>
#include <MRPT/MRML/CObservationStereoImages.h>
#include <MRPT/MRVL/CStereoGrabber_Bumblebee.h>
#include <MRPT/MRVL/VisionUtils.h>
#include <MRPT/UTILS/SYNCH.h>

#include <MRPT/config.h>

#if MRPT_HAS_BUMBLEBEE

#include <digiclops.h>
#include <triclops.h>

#endif

namespace MRVL
{
	class CStereoServerBumblebee
	{
	private:
		// ****************** GENERAL MEMBERS *********************
		MRVL::CStereoGrabber_Bumblebee		*grabber;				// Auxiliar grabber for the class

		MRML::CObservationStereoImages		*preImgs;				// Previous images of the camera
		MRML::CObservationStereoImages		*curImgs;				// Current images of the camera

		vector_float						pvX, pvY, pvZ;			// Vectors for the coordinates of the 3D cloud.
		vector_float						cvX, cvY, cvZ;			// Vectors for the coordinates of the 3D cloud.

		MRML::CObservationVisualLandmarks	preGrid;				// Previous map 3D computed by Bumblebee software
		MRML::CObservationVisualLandmarks	curGrid;				// Current map 3D computed by Bumblebee software

		MRML::CObservationVisualLandmarks	tmpGrid;				// Temporal copy of the current map 3D computed by Bumblebee software

		// ****************** TRACKING *********************
		CKanadeLucasTomasi::TKLTFeatureList	KLTListL, KLTListR;		// Tracked features
		unsigned int						nStereoImages;			// Indicates if tracking is possible or not (there are two pairs of stereo images available)
		unsigned int						nIter;					// Number of iterations for tracking
		UTILS::TTimeStamp					lastTS;					// Timestamp of the current observation

		// ****************** OBSTACLE AVOIDANCE *********************
		MRVL::TROI							safetyZone;				// Safety Zone of the robot
		bool								alert;					// Safety Zone violation
		UTILS::TTimeStamp					lastTSGrid;				// Timestamp of the grid observation (for obstacle avoidance)
		float								safetyRange;			// Minimum distance allowed

		// ****************** CRITICAL SECTIONS *********************
		UTILS::SYNCH::CCriticalSection		csImgs;					// Images
		UTILS::SYNCH::CCriticalSection		csGrid;					// Grid
		UTILS::SYNCH::CCriticalSection		csList;					// List of tracked features
		UTILS::SYNCH::CCriticalSection		csGrabber;				// Stereo Grabber
		UTILS::SYNCH::CCriticalSection		csVec;					// Vectors

	public:
		// ** Methods **
		// Default constructor
		CStereoServerBumblebee( int cameraIndex = 0, unsigned int resolutionX = 320, unsigned int resolutionY = 240 );
		// Destructor
		virtual ~CStereoServerBumblebee();

		// -- Images --
		// These are BABEL monitors
		// Capture
		void  monitorCaptureImgs();
		void  monitorCaptureGrid();
		void  monitorCaptureImgsAndGrid();

		// Processing
		void  monitorProcessTracking();
		void  monitorObstacleAvoidance();

		void  captureImgsAndObs();

		// Get a pair of images
		void  getImgs( MRML::CObservationStereoImages &outImgs );
		void  getObs( MRML::CObservationVisualLandmarks &outObs );

		void  getImgsAndObs( MRML::CObservationStereoImages &outImgs, MRML::CObservationVisualLandmarks &outGrid );

		//// -- Grid --
		//// Get grid in all the 3D space
		//void  getGrid3D( MRML::CObservationVisualLandmarks &outObs );
		//// Get grid with ROI
		//void  getGrid3D( TROI ROI, MRML::CObservationVisualLandmarks &outObs );

		//// -- Both --
		//// Get both Stereo Images and Grid in all the 3D space
		//void  getBothImagesAndGrid( MRML::CObservationStereoImages &outImgs, MRML::CObservationVisualLandmarks &outObs );
		//// Get both Stereo Images and Grid with ROI
		//void  getBothImagesAndGrid( TROI ROI, MRML::CObservationStereoImages &outImgs, MRML::CObservationVisualLandmarks &outObs );

	}; // end of class
} // end of namespace

#endif
