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

#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/MRML/CObservation.h>
#include <MRPT/MRML/CPose3D.h>
#include <MRPT/MRML/CLandmark.h>

namespace MRML
{
	/** This observation represents a number of range-bearing value pairs, each one for a detected landmark, which optionally can have identification IDs.
	 *  This class can manage sensors that detect landmarks in a 2D plane (e.g. a laser scanner) or in the 3D space (e.g. a camera). There are
	 *  two direction angles: yaw (azimuth) and pitch (negative elevation). For 2D sensors, the pitch must be always set to 0.
	 *
	 * \sa CObservation
	 */
	class CObservationBearingRange : public CObservation
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CObservationBearingRange )

	 public:
		/** Default constructor.
		 */
		CObservationBearingRange( );

		 /** Information about the sensor: Ranges, in meters
		  */
		float	minSensorDistance, maxSensorDistance;

		/** Information about the sensor: The "field-of-view" of the sensor, in radians (for both yaw & pitch).
		  */
		float fieldOfView;

		/** The position of the sensor on the robot.
		  */
		CPose3D		sensorLocationOnRobot;

		/** Each one of the measurements:
			*/
		struct TMeasurement
		{
			/** The sensed landmark distance, in meters.
			  */
			float	range;

			/** The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevation).
			  * See MRML::CPose3D for a definition of the 3D angles.
			  */
			float	yaw,pitch;

			/** The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark.
			  */
			int32_t	landmarkID;
		};

		/** The list of observed ranges:
		  */
		std::deque<TMeasurement> sensedData;


		 /** Implements the virtual method in charge of finding the likelihood between this
		  *   and another observation, probably only of the same derived class. The operator
		  *   may be asymmetric.
		  *
		  * \param anotherObs The other observation to compute likelihood with.
		  * \param anotherObsPose If known, the belief about the robot pose when the other observation was taken can be supplied here, or NULL if it is unknown.
		  *
		  * \return Returns a likelihood measurement, in the range [0,1].
		  *	\exception std::exception On any error, as another observation being of an invalid class.
		  */
		 float  likelihoodWith( const CObservation *anotherObs, const CPosePDF *anotherObsPose = NULL ) const;


		 /** Prints out the contents of the object.
		   */
		 void  debugPrintOut();

		/** A general method to retrieve the sensor pose on the robot.
		  *  Note that most sensors will return a full (6D) CPose3D, but see the derived classes for more details or special cases.
		  * \sa setSensorPose
		  */
		void getSensorPose( CPose3D &out_sensorPose ) const { out_sensorPose = sensorLocationOnRobot; }


		/** A general method to change the sensor pose on the robot.
		  *  Note that most sensors will use the full (6D) CPose3D, but see the derived classes for more details or special cases.
		  * \sa getSensorPose
		  */
		void setSensorPose( const CPose3D &newSensorPose ) { sensorLocationOnRobot = newSensorPose; }


	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CObservationBearingRange )

} // End of namespace

#endif
