/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#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();

		CStereoServerBumblebee(const CStereoServerBumblebee & )  :
			grabber(NULL),
			preImgs(NULL),
			curImgs(NULL),
			pvX(), pvY(), pvZ(),
			cvX(), cvY(), cvZ(),
			preGrid(),curGrid(),tmpGrid(),KLTListL(), KLTListR(),
			nStereoImages(0),nIter(0),lastTS(),safetyZone(),
			alert(false),lastTSGrid(),safetyRange(),
			csImgs(),csGrid(),csList(),csGrabber(),csVec()
		{
			THROW_EXCEPTION("This class cannot be copied.");
		}

		CStereoServerBumblebee & operator =(const CStereoServerBumblebee&) { THROW_EXCEPTION("This class cannot be copied."); }

		// -- 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
