/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/utils/utils_defs.h>
#include <mrpt/math/math_frwds.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/utils/TMatchingPair.h>
#include <mrpt/tfest/link_pragmas.h>
#include <mrpt/poses/poses_frwds.h>
#include <mrpt/tfest/indiv-compat-decls.h>

namespace mrpt
{
	/** Functions for estimating the optimal transformation between two frames of references given measurements of corresponding points.
	 * \sa mrpt::slam::CICP
	 * \ingroup mrpt_tfest_grp
	 */
	namespace tfest
	{
		/** \addtogroup mrpt_tfest_grp
		  * @{ */

		/** Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
		  *  The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other} \f$, that is, the 
		  *  transformation of frame `other` with respect to `this`.
		  *
		  *  \image html tfest_frames.png
		  *
		  * \param[in] in_correspondences The set of correspondences.
		  * \param[out] out_transformation The pose that minimizes the mean-square-error between all the correspondences.
		  * \param[out] out_estimateCovariance If provided (!=NULL) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper http://www.mrpt.org/Paper:Occupancy_Grid_Matching)
		  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
		  * \sa robustRigidTransformation
		  *
		  * \note Reference for covariance calculation: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps", Robotica, 2013. http://dx.doi.org/10.1017/S0263574712000732
		  * \note [New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation()
		  * \note This function is hand-optimized for SSE2 architectures (if SSE2 is enabled from CMake)
		  * \sa se3_l2, se2_l2_robust
		  * \ingroup sse_optimizations
		  * \ingroup mrpt_tfest_grp
		  */
		bool TFEST_IMPEXP se2_l2(
			const mrpt::utils::TMatchingPairList  & in_correspondences,
			mrpt::math::TPose2D                   & out_transformation,
			mrpt::math::CMatrixDouble33           * out_estimateCovariance = NULL );

		/** \overload */
		bool TFEST_IMPEXP se2_l2(
			const mrpt::utils::TMatchingPairList  & in_correspondences,
			mrpt::poses::CPosePDFGaussian         & out_transformation );


		/** Parameters for se2_l2_robust(). See function for more details */
		struct TFEST_IMPEXP TSE2RobustParams
		{
			unsigned int  ransac_minSetSize; //!< (Default=3)
			unsigned int  ransac_maxSetSize; //!< (Default = 20)
			double        ransac_mahalanobisDistanceThreshold; //!< (Default = 3.0)
			unsigned int  ransac_nSimulations; //!< (Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations, such as a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model
			/** (Default = true)  If true, the weight of Gaussian modes will be increased when an exact match in the
			*   subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the
			*   resulting X,Y,PHI means. Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi */
			bool          ransac_fuseByCorrsMatch;
			double        ransac_fuseMaxDiffXY; //!< (Default = 0.01)
			double        ransac_fuseMaxDiffPhi; //!< (Default=0.1degree) (In radians)
			bool          ransac_algorithmForLandmarks; //!< (Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
			double        probability_find_good_model; //!< (Default = 0.999) See parameter ransac_nSimulations. When using `probability_find_good_model`, the minimum number of iterations can be set with `ransac_min_nSimulations`
			unsigned int  ransac_min_nSimulations; //!< (Default = 1500) See parameter probability_find_good_model
			double        max_rmse_to_end;  //!< Stop searching for solutions when the RMSE of one solution is below this threshold. Special value "0" means "auto", which employs "2*normalizationStd".
			bool          verbose; //!< (Default=false)

			/** If provided, this user callback will be invoked to determine the individual compatibility between each potential pair 
			  * of elements. Can check image descriptors, geometrical properties, etc.
			  * \return Must return true if the pair is a potential match, false otherwise.
			  */
			//std::function<bool(TPotentialMatch)>  user_individual_compat_callback; // This could be used in the future when we enforce C++11 to users...
			TFunctorCheckPotentialMatch  user_individual_compat_callback;
			void * user_individual_compat_callback_userdata; //!< User data to be passed to user_individual_compat_callback()

			/** Default values */
			TSE2RobustParams() :
				ransac_minSetSize( 3 ),
				ransac_maxSetSize( 20 ),
				ransac_mahalanobisDistanceThreshold( 3.0 ),
				ransac_nSimulations( 0 ),
				ransac_fuseByCorrsMatch( true ),
				ransac_fuseMaxDiffXY(  0.01 ),
				ransac_fuseMaxDiffPhi(  mrpt::utils::DEG2RAD(0.1) ),
				ransac_algorithmForLandmarks( true ),
				probability_find_good_model( 0.999 ),
				ransac_min_nSimulations( 1500 ),
				max_rmse_to_end(0),
				verbose(false),
				user_individual_compat_callback(NULL),
				user_individual_compat_callback_userdata(NULL)
			{
			}
		};

		/** Output placeholder for se2_l2_robust() */
		struct TFEST_IMPEXP TSE2RobustResult
		{
			mrpt::poses::CPosePDFSOG        transformation; //!< The output as a set of transformations (sum of Gaussians)
			mrpt::utils::TMatchingPairList  largestSubSet; //!< the largest consensus sub-set
			unsigned int                    ransac_iters;  //!< Number of actual iterations executed 

			TSE2RobustResult() : ransac_iters(0) { }
		};


		/** Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
		  * This method implements a RANSAC-based robust estimation, returning a probability distribution over all the posibilities as a Sum of Gaussians.
		  *
		  *  The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other} \f$, that is, the 
		  *  transformation of frame `other` with respect to `this`.
		  *
		  *  \image html tfest_frames.png
		  *
		  *  The technique was described in the paper:
		  *    - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps", Robotica, 2013. http://dx.doi.org/10.1017/S0263574712000732
		  *
		  * This works as follows:
		  * - Repeat "ransac_nSimulations" times:
		  * 	- Randomly pick TWO correspondences from the set "in_correspondences".
		  * 	- Compute the associated rigid transformation.
		  * 	- For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
		  * 		- If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set"
		  * 		- If not, do not add it.
		  *
		  *  For more details refer to the tutorial on <a href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching methods</a>.
		  * \param[in] in_normalizationStd The <b>standard deviation</b> (not variance) of landmarks/points/features being matched in X,Y. Used to normalize covariances returned as the SoG. (Refer to paper)
		  * 
		  * <b>NOTE</b>: Parameter `ransac_maxSetSize` should be set to `in_correspondences.size()` to make sure that every correspondence is tested for each random permutation.
		  *
		  * \return True upon success, false if no subset was found with the minimum number of correspondences.
		  * \note [New in MRPT 1.3.0] This function replaces mrpt::scanmatching::robustRigidTransformation()
		  * \sa se3_l2, se2_l2_robust
		  */
		bool TFEST_IMPEXP se2_l2_robust(
			const mrpt::utils::TMatchingPairList &in_correspondences,
			const double               in_normalizationStd,
			const TSE2RobustParams   & in_ransac_params,
			TSE2RobustResult         & out_results
			);

		/** @} */  // end of grouping
	}
} // End of namespace
