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

#include <MRPT/UTILS/CSerializable.h>
#include <MRPT/UTILS/CMatrix.h>
#include <MRPT/UTILS/SystemUtils.h>
#include <MRPT/MRML/CPointPDFGaussian.h>
#include <MRPT/MRML/CPoint3D.h>

namespace MRML
{
	/** The class for storing "landmarks" (visual or laser-scan-extracted features,...)
	  * \sa CLandmarksMap
	  */
	class CLandmark : public UTILS::CSerializable
	{
		// This must be added to any CSerializable derived class:
		DEFINE_SERIALIZABLE( CLandmark )

	public:
		/** The type for the IDs of landmarks.
		  */
		typedef	int64_t TLandmarkID;

		/** The types of visual landmarks in a CLandmarkVisual class
		  * \sa descriptor
		  */
		enum TLandmarkType
		{
			/** Visual landmarks:
			  */
			vlColor = 0,
			vlSIFT,

			/** Generic landmarks:
			  */
		    glOccupancy,
			glPanoramicDescriptor,
			glBeacon
		};

		/** Its probabilistic 3D location
		  */
		float	pose_mean_x,pose_mean_y,pose_mean_z;
		float	pose_cov_11,pose_cov_22,pose_cov_33,pose_cov_12,pose_cov_13,pose_cov_23;

		/** The "normal" to the landmark, i.e. a unitary 3D vector towards the viewing direction, or a null vector if not applicable
		  */
		float	normal_x,normal_y,normal_z;

		/** Returns the pose as an object:
		  */
		void 	getPose( CPointPDFGaussian	*p ) const;

		/** Sets the pose from an object:
		  */
		void 	setPose( CPointPDFGaussian	*p );

		/** Sets the descriptor1[0] and [1] to the matrix's number of rows and cols, respectively, and store in "descriptor2" its contents, row by row.
		  * \sa getDescriptorAsMatrix
		  */
		void  setDescriptorFromMatrix( CMatrix &m );

		/** Get the descriptors as a matrix (see setDescriptorFromMatrix)
		  */
		void  getDescriptorAsMatrix( CMatrix &m );

		/** Sets descriptor1[0]-[3] (and sets it to the proper size) to the given value as being a beacon ID
		  * \sa getDescriptorAsBeaconID
		  */
		void  setDescriptorFromBeaconID( unsigned int beaconID );

		/** Gets descriptor1[0]-[3] as being a beacon ID.
		  * \sa setDescriptorFromBeaconID
		  */
		unsigned int  getDescriptorAsBeaconID( ) const;

		/** The type of this landmark
		  */
		TLandmarkType		type;

		/** An ID for the landmark (see details next...)
		  *  This ID was introduced in the version 3 of this class (21/NOV/2006), and its aim is
		  *  to provide a way for easily establishing correspondences between landmarks detected
		  *  in sequential image frames. Thus, the management of this field should be:
		  *		- In 'servers' (classes/modules/... that detect landmarks from images): A different ID must be assigned to every landmark (e.g. a sequential counter), BUT only in the case of being sure of the correspondence of one landmark with another one in the past (e.g. tracking).
		  *		- In 'clients': This field can be ignored, but if it is used, the advantage is solving the correspondence between landmarks detected in consequentive instants of time: Two landmarks with the same ID <b>correspond</b> to the same physical feature, BUT it should not be expected the inverse to be always true.
		  *
		  * Note that this field is never fill out automatically, it must be set by the programmer if used.
		  */
		TLandmarkID			ID;

		/** The last time that this landmark was observed.
		  */
		UTILS::TTimeStamp	timestampLastSeen;

		/** The number of times that this landmark has been seen.
		  */
		uint32_t			seenTimesCount;

		/** The vector containing the feature descriptor, landmark type dependent:
		  *  - type = vlColor
				- descriptor1 = 1x3 matrix with RGB components in the range [0,255]
				- descriptor2 = Empty

		  *  - type = vlSIFT
				- descriptor1 = 1xN matrix with the SIFT descriptor (typ N=128)
				- descriptor2 = [0]=Orientation in radians, [1]=Scale

		  *  - type = glOccupancy
				- descriptor1 = Empty
				- descriptor2 = 1x2 matrix with the angular direction (theta) and distance (d) from the range scan.

		  *  - type = glPanoramicDescriptor
				- descriptor1 = [0] is the number of rows R, [1] is the number of columns C
				- descriptor2 = An RxC matrix, stored row by row (first row first), containing the "panoramic descriptor"

		  *  - type = glBeacon
				- descriptor1 = 1x4 vector: [0]-[3] is the beacon ID. Use in this way: (*((unsigned int*) &descriptor1[0])
				- descriptor2 = Not used

		  */
		std::vector<unsigned char>	descriptor1;
		std::vector<float>			descriptor2;

		/** Default constructor
		  */
		CLandmark();

		/** Virtual destructor
		  */
		virtual ~CLandmark();

	protected:
		/** Auxiliary variable
		  */
		static TLandmarkID		m_counterIDs;

	}; // End of class definition

	DEFINE_SERIALIZABLE_POST( CLandmark )

} // End of namespace

#endif
