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

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


namespace MRML
{

	/** Declares a class derived from "CObservation" that
	       encapsules a 2D range scan measurement (typically from a laser scanner).
		   This is prepared for accepting 180deg,360deg or any other aperture scan,
		   as well as resolutions of 0.5deg,0.25deg or any other.
	 *
	 * \sa CObservation
	 */
	class CObservation2DRangeScan : public CObservation
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CObservation2DRangeScan )

	public:
		class CAuxMapWrapper
		{
			CPointsMap			*auxPointsMap;
		public:
			CAuxMapWrapper() :auxPointsMap(NULL) { }
			CAuxMapWrapper(const CAuxMapWrapper &o) : auxPointsMap(NULL) { }
			CAuxMapWrapper & operator =(const CAuxMapWrapper &o) { clear(); return *this; }

			~CAuxMapWrapper() { clear(); }

			void clear() { if (auxPointsMap) delete auxPointsMap; auxPointsMap=NULL; }

			CPointsMap	*get() { return auxPointsMap; }
			const CPointsMap	*get() const { return auxPointsMap; }

			void set(CPointsMap	*m) { if (auxPointsMap) delete auxPointsMap;  auxPointsMap = m; }

		};

	protected:
		CAuxMapWrapper	m_auxMap;

	 public:
		/** Default constructor
		 */
		CObservation2DRangeScan( );

		/** Destructor
		 */
		~CObservation2DRangeScan( );

		/** The range values of the scan, in meters.
		  */
		vector_float	    scan;

		/** It's false (=0) on no reflected rays, referenced to elements in "scan"
		  *  (Added in the streamming version #1 of the class)
		  */
		std::vector<char>	validRange;

		/** The aperture of the range finder, in radians (typically M_PI = 180 degrees).
		  */
		float				aperture;

		/** The scanning direction
		  */
		bool				rightToLeft;

		/** The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
		  */
		float				maxRange;

		/** The 6D pose of the sensor on the robot.
		  */
		CPose3D				sensorPose;

		/** The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
		  */
		float				stdError;

		/** The aperture of each beam, in radians, used to insert "thick" rays in the occupancy grid.
		  * (Added in the streamming version #4 of the class)
		  */
		float				beamAperture;

		/** If the laser gathers data by sweeping in the pitch/elevation angle, this holds the increment in "pitch" (=-"elevation") between the beginning and the end of the scan (the sensorPose member stands for the pose at the beginning of the scan).
		  */
		double				deltaPitch;

		/** Auxiliary members used to speed up some algorithms: it will be NULL normally.
		  * \sa buildAuxPointsMap
		  */
		const CPointsMap	*getAuxPointsMap() const { return m_auxMap.get(); }

		/** Returns an auxiliary points map built from the scan; it is actually build at the first call to this method, and only once.
		  * \param options Can be NULL to use default point maps' insertion options, or a valid pointer to override some params.
		  * \sa auxPointsMap
		  */
		const CPointsMap	*buildAuxPointsMap( CPointsMap::TInsertionOptions *options = NULL ) const;

		/** Return true if the laser scanner has an absolute value of "pitch" and "roll" less or equal to the given tolerance (in rads, default=0) (with the normal vector either upwards or downwards).
		  */
		bool 		isPlanarScan(const double &tolerance = 0) const;

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

		/** 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 = sensorPose; }


		/** 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 ) { sensorPose = newSensorPose; }

	}; // End of class def.

	DEFINE_SERIALIZABLE_POST( CObservation2DRangeScan )

} // End of namespace

#endif
