/*---------------------------------------------------------------
	FILE: VisionUtils.cpp
	USE: See doxygen doc.

   Part of the MRPT Library
   ISA - Universidad de Malaga - http://www.isa.uma.es
  ---------------------------------------------------------------*/
#ifndef VisionUtils_H
#define VisionUtils_H

// Avoid excesive compilation dependencies.
#include <MRPT/MRML/CMetricMap.h>

namespace MRML
{
	class CLandmarksMap;
	class CObservationVisualLandmarks;
	class CPoint3D;
}

#include <MRPT/MRVL/CKanadeLucasTomasi.h>

#include <MRPT/UTILS/CMRPTImage.h>
#include <MRPT/UTILS/MRPT_math.h>
#include <MRPT/config.h>

namespace MRVL
{

	/** Landmark ID
	  */
	typedef	 uint64_t TLandmarkID;

	/** Parameters associated to a stereo system
	  */
	struct TStereoSystemParams
	{
		/** Method for propagating the feature's image coordinate uncertainty into 3D space. Default value: Prop_Linear
	      */
		enum TUnc_Prop_Method
		{
			/** Linear propagation of the uncertainty
			  */
			Prop_Linear = -1,
			/** Uncertainty propagation through the Unscented Transformation
			  */
			Prop_UT,
			/** Uncertainty propagation through the Scaled Unscented Transformation
			  */
			Prop_SUT
		};

		TUnc_Prop_Method uncPropagation;

		/** Intrinsic parameters
		  */
		CMatrix		K;
		/** Baseline. Default value: baseline = 0.119f;	[Bumblebee]
		  */
		float		baseline;
		/** Standard deviation of the error in feature detection. Default value: stdPixel = 1
		  */
		float		stdPixel;
		/** Standard deviation of the error in disparity computation. Default value: stdDisp = 1
		  */
		float		stdDisp;
		/** Maximum allowed distance. Default value: maxZ = 20.0f
		  */
		float		maxZ;
		/** Maximum allowed distance. Default value: minZ = 0.5f
		  */
		float		minZ;
		/** Maximum allowed height. Default value: maxY = 3.0f
		  */
		float		maxY;
		/** K factor for the UT. Default value: k = 1.5f
		  */
		float		factor_k;
		/** Alpha factor for SUT. Default value: a = 1e-3
		  */
		float		factor_a;
		/** Beta factor for the SUT. Default value: b = 2.0f
		  */
		float		factor_b;

		/** Parameters initialization
		  */
		TStereoSystemParams()
		{
			K.setSize(3,3);
			K.fill(0);

			baseline = 0.119f;	// Bumblebee
			stdPixel = 1;
			stdDisp  = 1;
			maxZ	 = 20.0f;	// Indoor
			minZ	 = 0.5f;	// Indoor
			maxY	 = 3.0f;	// Indoor

			uncPropagation = Prop_Linear;

			factor_k = 1.5f;
			factor_a = 1e-3f;
			factor_b = 2.0f;
		}
	};

	/** A descriptor of a SIFT feature:
	  * \sa TSIFTFeature, MRVL::VisionUtils::computeSiftFeatures
	  */

	struct TSIFTFeature
	{
		/** Subpixel position on the image
			*/
		float						x, y;
		float						orientation;
		float						scale;
		std::vector<unsigned char>	descriptor;

		/**** FAMD ****/
		TLandmarkID					ID;
		/**** END FAMD ***/
	};

	// ******************************** FAMD ****************************************************
	struct TROI
	{
		// Constructors
		TROI();
		TROI(float x1, float x2, float y1, float y2, float z1, float z2);

		// Members
		float	xMin;
		float	xMax;
		float	yMin;
		float	yMax;
		float	zMin;
		float	zMax;
	};
	// **************************** END FAMD ****************************************************

	/** A list of SIFT features, relative to a given image.
	  * \sa TSIFTFeatureList, MRVL::VisionUtils::computeSiftFeatures
	  */
	class TSIFTFeatureList : public std::deque<TSIFTFeature>
	{
	public:
		/** Saves the list to a text file
		  */
		void  saveToFile(char *fileName);

	};



	/** A namespace for utility functions related to computer vision, in the general sense.
	  */
	namespace VisionUtils
	{
		/**	Computes the correlation between this image and another one, encapsulating the openCV function cvMatchTemplate
		*   This implementation reduced computation time.
		* \param patch_img The "patch" image, which must be equal, or smaller than "this" image. This function supports gray-scale (1 channel only) images.
		* \param u_search_ini The "x" coordinate of the search window.
		* \param v_search_ini The "y" coordinate of the search window.
		* \param u_search_size The width of the search window.
		* \param v_search_size The height of the search window.
		* \param u_max The u coordinate where find the maximun cross correlation value.
		* \param v_max The v coordinate where find the maximun cross correlation value
		* \param max_val The maximun value of cross correlation which we can find
		*  Note: By default, the search area is the whole (this) image.
		* (by AJOGD @ MAR-2007)
		* \sa cross_correlation
		*/
		void  openCV_cross_correlation(
												const CMRPTImage	&img,
												const CMRPTImage	&patch_img,
												int					&u_max,
												int					&v_max,
												double				&max_val,
												int					u_search_ini=-1,
												int					v_search_ini=-1,
												int					u_search_size=-1,
												int					v_search_size=-1);

		/**	Invert an image using OpenCV function
		*
		*/
		void  flip(CMRPTImage		&img);

		/** Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates, and the camera intrinsic coordinates.
		  *  \param x Pixels coordinates, from the top-left corner of the image.
		  *  \param y Pixels coordinates, from the top-left corner of the image.
		  *  \param A The 3x3 intrinsic parameters matrix for the camera.
		  *
		  * \sa buildIntrinsicParamsMatrix, defaultIntrinsicParamsMatrix
		  */
		MRML::CPoint3D  pixelTo3D(float x,float y, CMatrix &A);

		/** Builds the intrinsic parameters matrix A from parameters:
		  * \param focalLengthX The focal length, in X (horizontal) pixels
		  * \param focalLengthY The focal length, in Y (vertical) pixels
		  * \param centerX The image center, horizontal, in pixels
		  * \param centerY The image center, vertical, in pixels
		  *
		  * <br>This method computes A=
		  <table>
		  <tr><td>f</td><td>0</td><td>cX</td> </tr>
		  <tr><td>0</td><td>f</td><td>cY</td> </tr>
		  <tr><td>0</td><td>0</td><td>1</td> </tr>
		  </table>
		  * \sa defaultIntrinsicParamsMatrix, pixelTo3D
		  */
		CMatrix  buildIntrinsicParamsMatrix(
						float	focalLengthX,
						float	focalLengthY,
						float	centerX,
						float	centerY);

		/** Returns the stored, default intrinsic params matrix for a given camera:
		  * \param camIndex  Posible values are listed next.
		  * \param resolutionX The number of pixel columns
		  * \param resolutionY The number of pixel rows
		  *
		  * The matrix is generated for the indicated camera resolution configuration.
		  * The following table summarizes the current supported cameras and the values as
		  *  ratios of the corresponding horz. or vert. resolution:<br>

		  <center><table>
		  <tr>
		   <td><center><b>camIndex</b></center></td>
		   <td><center><b>Manufacturer</b></center></td>
		   <td><center><b>Camera model</b></center></td>
		   <td><center><b>fx</b></center></td>
		   <td><center><b>fy</b></center></td>
		   <td><center><b>cx</b></center></td>
		   <td><center><b>cy</b></center></td>
		  </tr>

		  <tr>
		   <td><center>0</center></td>
		   <td><center>Point Grey Research</center></td>
		   <td><center>Bumblebee</center></td>
		   <td><center>0.79345</center></td>
		   <td><center>1.05793</center></td>
		   <td><center>0.55662</center></td>
		   <td><center>0.52692</center></td>
		  </tr>

		  <tr>
		   <td><center>1</center></td>
		   <td><center>Sony</center></td>
		   <td><center>???</center></td>
		   <td><center>0.95666094</center></td>
		   <td><center>1.3983423f</center></td>
		   <td><center>0.54626328f</center></td>
		   <td><center>0.4939191f</center></td>
		  </tr>
		  </table>
		  </center>

		  * \sa buildIntrinsicParamsMatrix, pixelTo3D
		  */
		CMatrix  defaultIntrinsicParamsMatrix(
						unsigned int camIndex = 0,
						unsigned int resolutionX = 320,
						unsigned int resolutionY = 240 );

		/** Delete repeated features in a vector
		*/
		void  deleteRepeatedFeats( TSIFTFeatureList &list );


		/** Returns the maximum ID of the list ...
		  * ...
		  */
		int  getMaxID( CKanadeLucasTomasi::TKLTFeatureList &list );

		/** Computes ONLY the SIFT descriptors of a set of features ...
		  * ...
		  */
		void  computeSiftDescriptors( const CMRPTImage					&in_img,
												CKanadeLucasTomasi::TKLTFeatureList	&in_features,
												TSIFTFeatureList					&out_features);

		/** Computes a set of features with the SIFT algorithm...
		  * ...
		  */
		void  computeSiftFeatures( CMRPTImage			&in_img,
											 TSIFTFeatureList	&out_features,
											 int				wantedNumberOfFeatures = 150 );

		/** Computes a set of features with the SIFT algorithm with Lowe binary...
		  * ...
		  * /
		void  computeSiftFeatures( std::string img,
											 TSIFTFeatureList &features,
											 unsigned int nFeatures = 0); */

		/** Project to 3D space the set of matched features in a stereo vision framework...
		  * ...
		  */
		void  projectMatchedKLT(	MRVL::CKanadeLucasTomasi::TKLTFeatureList &KLTmatchL,
											MRVL::CKanadeLucasTomasi::TKLTFeatureList &KLTmatchR,
											TStereoSystemParams options,
											MRML::CLandmarksMap &landmarks );

		/** Project to 3D space the set of matched features in a stereo vision framework...
		  * ...
		  */
		void  projectMatchedSIFT( TSIFTFeatureList &SIFTmatchL,			// SIFT features in the left image
											TSIFTFeatureList &SIFTmatchR,			// SIFT features in the right image
											TStereoSystemParams options,
											MRML::CLandmarksMap &landmarks );		// Output map of 3D landmarks

		/** Match SIFT lists of points in a stereo vision framework...
		  * ...
		  */
		void  matchSIFT( TSIFTFeatureList &leftList, TSIFTFeatureList &rightList,
								    TSIFTFeatureList &outLeftList, TSIFTFeatureList &outRightList);

		/** Filter bad correspondences by distance
		  * ...
		  */
		void  filterBadCorrsByDistance( MRML::CMetricMap::TMatchingPairList &list,	// The list of correspondences
													unsigned int numberOfSigmas );				// Threshold


		/** Returns a new image where distortion has been removed.
		  * \param A The 3x3 intrinsic parameters matrix
		  * \param dist_coeffs The 1x4 vector of distortion coefficients
		  */
		void  correctDistortion(
				CMRPTImage		&in_img,
				CMRPTImage		&out_img,
				CMatrix			A,
				CMatrix			dist_coeffs );

		/** Transform HSV color components to RGB, all of them in the range [0,1]
		  * \sa rgb2hsv
		  */
		void  hsv2rgb(
			float	h,
			float	s,
			float	v,
			float	&r,
			float	&g,
			float	&b);

		/** Transform RGB color components to HSV, all of them in the range [0,1]
		  * \sa hsv2rgb
		  */
		void  rgb2hsv(
			float	r,
			float	g,
			float	b,
			float	&h,
			float	&s,
			float	&v );

		/** Computes the mean squared distance between a set of 3D correspondences
		  * ...
		  */
		double  computeMsd( const MRML::CMetricMap::TMatchingPairList &list,
									  const MRML::CPose3D &Rt );

		/** Transform two clouds of 3D points into a matched list of points
		  * ...
		  */
		void  cloudsToMatchedList( const MRML::CObservationVisualLandmarks &cloud1,
								   const MRML::CObservationVisualLandmarks &cloud2,
										 MRML::CMetricMap::TMatchingPairList &outList);

		/** Computes the main orientation of a set of points with an image (for using in SIFT-based algorithms)
		  * \param image (Input). The input image.
		  * \param x (Input). A vector containing the 'x' coordinates of the image points.
		  * \param y (Input). A vector containing the 'y' coordinates of the image points.
		  * \param orientation (Output). A vector containing the main orientation of the image points.
		  */
		float computeMainOrientation( const CMRPTImage &image,
														const unsigned int &x,
														const unsigned int &y );

	}	// End of NS

} // End of NS


#endif
