/* +---------------------------------------------------------------------------+
   |          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 CStereoGrabber_Bumblebee_H
#define CStereoGrabber_Bumblebee_H


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

#include <MRPT/config.h>

#if MRPT_HAS_BUMBLEBEE
	#include <digiclops.h>
	#include <triclops.h>
#endif


namespace MRVL
{
	/** A class for grabing stereo images from a "Bumblebee" camera
	  * NOTE:
	  *		- This class is only available when compiling MRPT with "#define MRPT_HAS_BUMBLEBEE 1" in the "config.h".
	  *		- You will need the "include" and "lib" directories of the vendor's proprietary software to be included in VC++ includes path.
	  */
	class CStereoGrabber_Bumblebee
	{
	protected:
#if MRPT_HAS_BUMBLEBEE
		TriclopsContext  m_triclops;            // triclops context
		DigiclopsContext m_digiclops;		    // Digiclops context.
		TriclopsInput    m_triclopsInput;		// first grabbed input
#endif

		/** If this has been correctly initiated
		*/
		bool		    m_bInitialized;

		/** Rectified images
		*/
		unsigned char*   m_pRectImageLeft,*m_pRectImageRight;

		/** Disparity image
		*/
		unsigned short*   m_pDispImage;

		unsigned int	  m_resolutionX, m_resolutionY;

	public:
		/** Constructor:
		*/
		CStereoGrabber_Bumblebee( int cameraIndex = 0, unsigned int resolutionX = 320, unsigned int resolutionY = 240 );

		CStereoGrabber_Bumblebee( const CStereoGrabber_Bumblebee& ) :
			m_bInitialized(false),
			m_pRectImageLeft(NULL),m_pRectImageRight(NULL),
			m_pDispImage(NULL),
			m_resolutionX(0), m_resolutionY(0)
		{
			THROW_EXCEPTION("This class cannot be copied.");
		}

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

		/** Destructor
		*/
		virtual ~CStereoGrabber_Bumblebee(void);

		/** Grab stereo images, process them, and return the computed data.
		 * \param out_observation The object to be filled with sensed data. Landmarks type is "vlColor"
		 *
		 *  NOTICE: That the member "CObservationVisualLandmarks::refCameraPose" must be
		 *           set on the return of this method, since we don't know here the robot physical structure.
		 *
		 * \return false on any error, true if all go fine.
		*/
		bool  getObservation( MRML::CObservationVisualLandmarks &out_observation);

		// ******************************** FAMD ****************************************************
		/** Grab stereo images, process them, and return the computed data.
		 * \param ROI Region of interest where the data will be computed
		 * \param out_observation The object to be filled with sensed data. Landmarks type is "vlColor"
		 *
		 *  NOTICE: That the member "CObservationVisualLandmarks::refCameraPose" must be
		 *           set on the return of this method, since we don't know here the robot physical structure.
		 *
		 * \return false on any error, true if all go fine.
		*/
		bool  getObservation(
					TROI ROI,
					MRML::CObservationVisualLandmarks &out_observation);
		// ****************************** END FAMD ***************************************************

		/** Grab stereo images, and return the pair of rectified images.
		 * \param out_observation The object to be filled with sensed data.
		 *
		 *  NOTICE: (1) That the member "CObservationVisualLandmarks::refCameraPose" must be
		 *                set on the return of this method, since we don't know here the robot physical structure.
		 *          (2) The images are already rectified.
		 *
		 * \return false on any error, true if all go fine.
		*/
		bool  getImagesPairObservation( MRML::CObservationStereoImages &out_observation);


		/** This is a merge method for "getImagesPairObservation" and "getObservation", which gets both kind of observations.
		  */
		bool  getBothObservation(
					MRML::CObservationVisualLandmarks	&out_observation,
					MRML::CObservationStereoImages		&out_observationStereo );

		// ******************************** FAMD ****************************************************
		/** This is a merge method for "getImagesPairObservation" and "getObservation", which gets both kind of observations.
		  * The ROI defines the Region of Interest for the Observation
		  */
		bool  getBothObservation(
					TROI								ROI,
					MRML::CObservationVisualLandmarks	&out_observation,
					MRML::CObservationStereoImages		&out_observationStereo );

		/** This is a fast merge method for "getImagesPairObservation" and "getObservation", which gets both kind of observations.
		  * The ROI defines the Region of Interest for the Observation
		  */
		bool  getBothObservation(
			vector_float						&vX,
			vector_float						&vY,
			vector_float						&vZ,
			MRML::CObservationStereoImages		&out_observationStereo );
		// ****************************** END FAMD ***************************************************


	};	// End of class

} // End of NS


#endif
