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

#include <MRPT/UTILS/CMRPTImage.h>
#include <MRPT/UTILS/CMRPTImageFloat.h>
#include <MRPT/MRVL/CKanadeLucasTomasi.h>
#include <MRPT/MRVL/CGaussianConvolutionKernel.h>
//#include <MRPT/MRVL/VisionUtils.h>

namespace MRVL
{
	/** This class integrates a Kanade-Lucas-Tomasi (KLT) features finder and tracker for gray-scale images.
	  * \sa MRVL
	  */
	class CKanadeLucasTomasi
	{
	public:

		typedef	uint64_t TKLTFeatureID;

		/** Used internally:
		  */
		enum TKLTFeatureStatus
		{
			statusKLT_IDLE = -1,
			statusKLT_OOB,
			statusKLT_SMALL_DET,
			statusKLT_LARGE_RESIDUE,
			statusKLT_MAX_RESIDUE,
			statusKLT_TRACKED,
			statusKLT_MAX_ITERATIONS
		};

		enum TKLTImplementation
		{
			KLT_imp_MRPT = -1,
			KLT_imp_OpenCV
		};

		/** An internal structure for storing a KLT feature.
		  */
		struct TKLTFeature
		{
			TKLTFeature(float x_ = 0, float y_ = 0, float val_ = 0) :
				status(statusKLT_IDLE),
				x(x_),
				y(y_),
				val(val_),
				ID(0)
			{
			}

			//void  TKLTTempFeature::operator=(const TSIFTFeature &feat )
			//{
			//	this->ID		= feat.ID;
			//	this->x			= feat.x;
			//	this->y			= feat.y;
			//	this->val		= 0;
			//	this->status	= statusKLT_IDLE;
			//}

			TKLTFeatureStatus	status;
			float				x,y;
			float				val;
			TKLTFeatureID		ID;
		};

		/** A list of KLT features
		  */
		typedef std::vector<TKLTFeature> TKLTFeatureList;
		void  saveToFile(const TKLTFeatureList &o, const char *fileName);

		enum TConsistencyCheck
		{
			ccDont = -1,	// -1 = don't evaluates the consistency
			ccTranslation,  //  0 = evaluates the consistency of features with translation mapping
			ccSimilarity,	//  1 = evaluates the consistency of features with similarity mapping
			ccAffine		//  2 = evaluates the consistency of features with affine mapping
		};

		// STORED IMAGES!!
		CMRPTImageFloat		im1, im2;

		/** Options for the KLT algorithm:
		*/
		struct TOptions
		{
			/** Initializer for default values
			*/
			TOptions();

			/** Verbose variable
			*/
			bool	verbose;

			/** Minimum number of tracked features
			*/
			unsigned int min_num_features;

			/** min distance b/w features
			*/
			int		mindist ;

			int		window_width, window_height;

			/** whether to save most recent image to save time
				*/
			bool	sequentialMode;

			/** whether to smooth image before.
			 * can set to TRUE manually, but don't set to
			 * FALSE manually */
			bool	smoothBeforeSelecting;

			/** selecting features
			  *  whether to write internal images
              */
			bool	writeInternalImages;

			/* tracking features
			 * whether to normalize for gain and bias
             */
			bool	lighting_insensitive;

			/* Available, but hopefully can ignore */
			int		min_eigenvalue;		/* smallest eigenvalue allowed for selecting */
			float	min_determinant;	/* th for determining lost */
			float	min_displacement;	/* th for stopping tracking when pixel changes little */
			int		max_iterations;		/* th for stopping tracking when too many iterations */
			float	max_residue;		/* th for stopping tracking when residue is large */
			float	grad_sigma;
			float	smooth_sigma_fact;
			float	pyramid_sigma_fact;
			int		nSkippedPixels;		/* # of pixels skipped when finding features */
			int		borderx;			/* border in which features will not be found */
			int		bordery;
			int		nPyramidLevels;		/* computed from search_ranges */
			int		subsampling;		/* 		*/

			/* for affine mapping */
			int		affine_window_width, affine_window_height;

			/** whether to evaluates the consistency of features with affine mapping
				*/
			TConsistencyCheck	affineConsistencyCheck;

			int		affine_max_iterations;
			float	affine_max_residue;
			float	affine_min_displacement;
			float	affine_max_displacement_differ; /* th for the difference between the displacement calculated
								by the affine tracker and the frame to frame tracker in pel*/

			int		search_range;

			float	min_eigenvalue_thres;

			/** Image size (Default values: 640x480)
			*/
			unsigned int imgWidth, imgHeight;

			TKLTImplementation implement;

		protected:
			void	*pyramid_last;
			void	*pyramid_last_gradx;
			void	*pyramid_last_grady;

		} m_options;

		/********************************** FAMD ******************************************/

		/** Pyramid of images
		  */
		struct TKLTPyramid
		{
			TKLTPyramid() :
				subsampling(),
				nLevels(),
				imgs(),
				nCols(),
				nRows()
			{ }

			TKLTPyramid( int subsampling_, int nLevels_ ) :
				subsampling(subsampling_),
				nLevels(nLevels_),
				imgs(nLevels_),
				nCols(nLevels_),
				nRows(nLevels_)
			{
			}

			int								subsampling;
			int								nLevels;
			std::vector<CMRPTImageFloat>	imgs;
			std::vector<int>				nCols;
			std::vector<int>				nRows;
		};
		/******************************** END FAMD ****************************************/

		///** An internal structure for storing a KLT feature.
		//  */
		//struct TKLTFeature
		//{
		//	float	x,y;
		//	int		val;

		//	/* for affine mapping */
		//	CMRPTImageFloat	aff_img;
		//	CMRPTImageFloat	aff_img_gradx;
		//	CMRPTImageFloat aff_img_grady;
		//	float			aff_x;
		//	float			aff_y;
		//	float			aff_Axx;
		//	float			aff_Ayx;
		//	float			aff_Axy;
		//	float			aff_Ayy;
		//};

		/** Selects a set of good features in an image.
		  * \param inImg (input)
		  * \param features (output)
		  * \param nDesiredFeatures (op. input)
		  * \param omitPixels (op. input) -> 0 do not ommit this pixel, 1 omit this pixel
		  */
		void  selectGoodFeatures(
			const CMRPTImage	&inImg,
			TKLTFeatureList &features,
			unsigned int nDesiredFeatures = 0,
			CMatrix	*omitPixels = NULL);

		/** Tracks a set of features in an image.
		  */
		void  trackFeatures(
			const CMRPTImage	&inImg1,
			const CMRPTImage	&inImg2,
			TKLTFeatureList &features);

		/** Find more features in the image (excluding the ones into the inlist)
		  */
		void  findMoreFeatures(
			const CMRPTImage &img,
			TKLTFeatureList &inList,
			TKLTFeatureList &outList,
			unsigned int nDesiredFeats = 0);

		void  rowChecking(
			TKLTFeatureList &leftList,
			TKLTFeatureList &rightList,
			float threshold = 0.0);


		TKLTFeatureStatus  trackFeature(
			float xloc, float yloc,													// Location of the center of the window in image 1
			float &xlocout, float &ylocout,											// Start location of search in image 2
			CMRPTImageFloat &im1, CMRPTImageFloat &gradx1, CMRPTImageFloat &grady1,	// First Images
			CMRPTImageFloat &im2, CMRPTImageFloat &gradx2, CMRPTImageFloat &grady2,	// Second Image
			int width, int height,													// Window size
			int max_iterations,														// Options and stop conditions
			float min_determinant,
			float min_displacement,
			float max_residue);


		void replaceLostFeatures(
			CMRPTImageFloat	&img,
			TKLTFeatureList &features);

		/** Constructor
		  */
		CKanadeLucasTomasi( TOptions *options = NULL );

	private:
		/*--------- Utilities ------------ */

		/** Returns the number of remaining features...
		  */
		void  countRemainingFeatures( TKLTFeatureList &features );

		/** Create the pyramid of images ...
		  */
		void  createPyramid( int nCols, int nRows, int subsampling, int nLevels, TKLTPyramid &pyramid );

		/** Compute the pyramid of images ...
		  */
		void  computePyramid( CMRPTImageFloat &img, float sigma, TKLTPyramid &pyramid );

		/** Changes the search range.
		  */
		void  changeTCPyramid( int searchRange );

		/** Updates the TC border...
		  */
		void  updateTCBorder( );

		/** Stop the sequential mode.
		  */
		void  stopSequentialMode();

		/** Computes a smooth sigma.
		  */
		float  computeSmoothSigma();

		void  computeIntensityDifference( CMRPTImageFloat &im1, CMRPTImageFloat &im2, // Images
													float xloc, float yloc,						// Location in image 1
													float xlocout, float ylocout,				// Location in image 2
													int width, int height,						// Window size
													CMRPTImageFloat &imgdiff );					// Output image

		void  computeGradientSum( CMRPTImageFloat &gradx1, CMRPTImageFloat &grady1,	// Gradient images 1
										   CMRPTImageFloat &gradx2, CMRPTImageFloat &grady2,	// Gradient images 2
										   float xloc, float yloc,								// Location in image 1
										   float xlocout, float ylocout,						// Location in image 2
										   int width, int height,								// Window size
										   CMRPTImageFloat &gradx, CMRPTImageFloat &grady);		// Output image

		void  compute2x2GradientMatrix( CMRPTImageFloat &gradx, CMRPTImageFloat &grady,// Gradient images
												 int width, int height,							// Window size
												 float &gxx, float &gxy, float &gyy );			// Output values


		void  compute2x1ErrorVector( CMRPTImageFloat &imgdiff,						// Difference image
											  CMRPTImageFloat &gradx, CMRPTImageFloat &grady,	// Gradient images
											  int width, int height,							// Window size
											  float &ex, float &ey );							// Output values

		TKLTFeatureStatus  solveDisplacementEquation( float gxx, float gxy, float gyy,// Gradient matrix values
																float ex, float ey,				// Error vector values
																float min_determinant,			// Minimum determinant allowed
																float &dx, float &dy );			// Output displacement values

		float  sumAbsFloatWindow( CMRPTImageFloat &imgdiff,		// Input image
											int width, int height );		// Window size

		float  interpolate( float xloc, float yloc,	// Location
							 CMRPTImageFloat &im1 );				// Image



	};	// End of class "CKanadeLucasTomasi"

} // End of namespace


#endif
