/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/containers/yaml.h>
#include <mrpt/img/CImage.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/vision/TKeyPoint.h>
#include <mrpt/vision/types.h>

#include <memory>  // for unique_ptr

namespace mrpt::vision
{
/** \addtogroup vision_tracking Feature detection and tracking
 *  \ingroup mrpt_vision_grp
 *   @{  */

/** A virtual interface for all feature trackers, implementing the part of
 * feature tracking that is common to any specific tracker implementation.
 *   This class provides a quite robust tracking of features, avoiding as many
 * outliers as possible but not all of them:
 *    more robust tracking would require application-specific information and
 * could be done in a number of very different approaches,
 *    so this class will not try to do any kind of RANSAC or any other advanced
 * outlier rejection; instead, it should
 *    be done by the users or the classes that employ this class.
 *
 *   The basic usage of this class is as follows:
 *    \code
 *       CFeatureTracker_KL    tracker;  // Note: CFeatureTracker_KL is the
 * most robust implementation for now.
 *       tracker.extra_params["add_new_features"] = 1;  // Enable detection of
 * new features, not only tracking
 *       tracker.extra_params[...] = ...
 *       // ....
 *       TKeyPointList theFeats;  // The list of features
 *       mrpt::img::CImage  previous_img, current_img;
 *
 *       while (true) {
 *           current_img = ... // Grab new image.
 *           if ( previous_img_is_ok )
 *               tracker.trackFeatures(previous_img, current_img, theFeats);
 *           previous_img = current_img;
 *       }
 *    \endcode
 *
 *  Below follows the list of optional parameters for "extra_params" which can
 * be set
 *  and will be understood by this base class for any specific tracker
 * implementation.
 *  Note that all parameters are double's, but boolean flags are emulated by
 * the values 0.0 (false) and 1.0 (true).
 *
 *  List of parameters:
 * <table border="1" >
 *   <tr><td align="center" > <b>Parameter name</b>  </td>  <td align="center"
 * > <b>Default value</b> </td> <td align="center" > <b>Comments</b> </td> </tr>
 *   <tr><td align="center" > add_new_features  </td>  <td align="center" > 0
 * </td>
 *      <td> If set to "1", the class will not only track existing features,
 * but will also perform (after doing the actual tracking) an efficient
 *            search for new features with the FAST detector, and will add them
 * to the passed feature list if they fulfill a set of restrictions,
 *            as stablished by the other parameters (see
 * <i>add_new_feat_min_separation</i>,<i>add_new_feat_max_features</i>,<i>minimum_KLT_response_to_add</i>).
 *        </td> </tr>
 *   <tr><td align="center" > add_new_feat_min_separation  </td>  <td
 * align="center" > 15 </td>
 *      <td> If <i>add_new_features</i>==1,  this is the minimum separation (in
 * pixels) to any other (old, or new) feature for it
 *             being considered a candidate to be added.
 *         </td> </tr>
 *   <tr><td align="center" > desired_num_features_adapt  </td>  <td
 * align="center" > (img_width*img_height)/512 </td>
 *      <td> If <i>add_new_features</i>==1, the threshold of the FAST(ER)
 * feature detector is dynamically adapted such as the number of
 *        raw FAST keypoints is around this number. This number should be much
 * higher than the real desired numbre of features, since this
 *        one includes many features concentrated in space which are later
 * discarded for the minimum distance.
 *         </td> </tr>
 *   <tr><td align="center" > desired_num_features </td>  <td align="center" >
 * 100 </td>
 *      <td> If <i>add_new_features</i>==1, the target number of the patch
 * associated to each feature will be updated with every N'th frame. </td> </tr>
 *   <tr><td align="center" > add_new_feat_patch_size  </td>  <td
 * align="center" > 11 </td>
 *      <td> If <i>add_new_features</i>==1,  for each new added feature, this
 * is the size of the patch to be extracted around the keypoint (set to 0 if
 * patches are not required at all).
 *          </td> </tr>
 *   <tr><td align="center" > minimum_KLT_response_to_add  </td>  <td
 * align="center" > 10 </td>
 *      <td> If <i>add_new_features</i>==1, this sets the minimum KLT response
 * of candidate FAST features to be added in each frame, if they also fulfil the
 * other restrictions (e.g. min.distance).
 *         </td> </tr>
 *   <tr><td align="center" > check_KLT_response_every  </td>  <td
 * align="center" > 0 </td>
 *      <td> If >0, it will compute the KLT response at each feature point
 * every <i>N</i> frames
 *            and those below <i>minimum_KLT_response</i> will be marked as
 * "lost" in their "track_status" field.
 *        </td> </tr>
 *   <tr><td align="center" > minimum_KLT_response  </td>  <td align="center" >
 * 5 </td>
 *      <td> See explanation of <i>check_KLT_response_every</i>.
 *        </td> </tr>
 *   <tr><td align="center" > KLT_response_half_win  </td>  <td align="center"
 * > 4 </td>
 *      <td> When computing the KLT response of features (see
 * <i>minimum_KLT_response</i> and <i>minimum_KLT_response_to_add</i>),
 *            the window centered at the point for its estimation will be of
 * size (2*W+1)x(2*W+1), with <i>W</i> being this parameter value.
 *       </td> </tr>
 *   <tr><td align="center" > update_patches_every  </td>  <td align="center" >
 * 0 </td>
 *      <td> If !=0, the patch associated to each feature will be updated with
 * every N'th frame. </td> </tr>
 *   <tr><td align="center" > remove_lost_features  </td>  <td align="center" >
 * 0 </td>
 *      <td> If !=0, out-of-bound features or those lost while tracking, will
 * be automatically removed from the list of features.
 *            Otherwise, the user will have to manually remove them by checking
 * the track_status field. </td> </tr>
 * </table>
 *
 *  This class also offers a time profiler, disabled by default (see
 * getProfiler and enableTimeLogger).
 *
 * \sa CFeatureTracker_KL, the example application "track-video-features".
 */
struct CGenericFeatureTracker
{
	/** Optional list of extra parameters to the algorithm. */
	mrpt::containers::yaml extra_params;

	/** Default ctor */
	inline CGenericFeatureTracker() {}

	/** Ctor with extra parameters */
	inline CGenericFeatureTracker(const mrpt::containers::yaml& extraParams)
		: extra_params(extraParams)
	{
	}

	/** Dtor */
	virtual ~CGenericFeatureTracker() = default;

	/** Perform feature tracking from "old_img" to "new_img", with a (possibly
	 *empty) list of previously tracked features "inout_featureList".
	 *  This is a list of parameters (in "extraParams") accepted by ALL
	 *implementations of feature tracker (see each derived class for more
	 *specific parameters).
	 *		- "add_new_features" (Default=0). If set to "1", new features will
	 *be
	 *also added to the existing ones in areas of the image poor of features.
	 * This method does:
	 *    - Convert old and new images to grayscale, if they're in color.
	 *    - Call the pure virtual "trackFeatures_impl" method.
	 *    - Implement the optional detection of new features if
	 *"add_new_features"!=0.
	 */
	void trackFeatures(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointList& inout_featureList);

	/** overload with subpixel precision */
	void trackFeatures(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointfList& inout_featureList);

	/** Returns a read-only reference to the internal time logger */
	inline const mrpt::system::CTimeLogger& getProfiler() const
	{
		return m_timlog;
	}
	/** Returns a reference to the internal time logger */
	inline mrpt::system::CTimeLogger& getProfiler() { return m_timlog; }
	/** Returns a read-only reference to the internal time logger */
	inline void enableTimeLogger(bool enable = true)
	{
		m_timlog.enable(enable);
	}

	/** Returns the current adaptive threshold used by the FAST(ER) detector to
	 * find out new features in empty areas */
	inline int getDetectorAdaptiveThreshold() const
	{
		return m_detector_adaptive_thres;
	}

	struct TExtraOutputInfo
	{
		/** In the new_img with the last adaptive threshold */
		size_t raw_FAST_feats_detected;
		/** The number of features which were deleted due to OOB, bad tracking,
		 * etc... (only if "remove_lost_features" is enabled) */
		size_t num_deleted_feats;
	};

	/** Updated with each call to trackFeatures() */
	TExtraOutputInfo last_execution_extra_info;

   protected:
	/** The tracking method implementation, to be implemented in children
	 * classes. */
	virtual void trackFeatures_impl(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointfList& inout_featureList);

	/** The tracking method implementation, to be implemented in children
	 * classes. */
	virtual void trackFeatures_impl(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointList& inout_featureList) = 0;

	/** the internal time logger, disabled by default. */
	mrpt::system::CTimeLogger m_timlog{false};

	/** This field is clared by \a trackFeatures() before calling \a
	 * trackFeatures_impl(), and
	 *   can be filled out with newly defected FAST(ER) features in the latter.
	 * If it's not the case, feats will be computed anyway if the user enabled
	 * the "add_new_features" option.
	 */
	mrpt::vision::TKeyPointList m_newly_detected_feats;

	/** Adapts the threshold \a m_detector_adaptive_thres according to the real
	 * and desired number of features just detected */
	void updateAdaptiveNewFeatsThreshold(
		const size_t nNewlyDetectedFeats, const size_t desired_num_features);

   private:
	/** for use when "update_patches_every">=1 */
	size_t m_update_patches_counter{0};
	/** For use when "check_KLT_response_every">=1 */
	size_t m_check_KLT_counter{0};
	/** For use in "add_new_features" == true */
	int m_detector_adaptive_thres{10};

	template <typename FEATLIST>
	void internal_trackFeatures(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		FEATLIST& inout_featureList);
};

using CGenericFeatureTrackerAutoPtr = std::unique_ptr<CGenericFeatureTracker>;

/** Track a set of features from old_img -> new_img using sparse optimal flow
 *(classic KL method).
 *
 *  See CGenericFeatureTracker for a more detailed explanation on how to use
 *this class.
 *
 *   List of additional parameters in "extra_params" (apart from those in
 *CGenericFeatureTracker) accepted by this class:
 *		- "window_width"  (Default=15)
 *		- "window_height" (Default=15)
 *		- "LK_levels" (Default=3) Number of pyramids to build for LK tracking
 *(this
 *parameter only has effects when tracking with CImage's, not with
 *CImagePyramid's).
 *		- "LK_max_iters" (Default=10) Max. number of iterations in LK tracking.
 *		- "LK_epsilon" (Default=0.1) Minimum epsilon step in interations of
 *LK_tracking.
 *		- "LK_max_tracking_error" (Default=150.0) The maximum "tracking error"
 *of
 *LK tracking such as a feature is marked as "lost".
 *
 *  \sa OpenCV's method cvCalcOpticalFlowPyrLK
 */
struct CFeatureTracker_KL : public CGenericFeatureTracker
{
	/** Default ctor */
	inline CFeatureTracker_KL() = default;
	/** Ctor with extra parameters */
	inline CFeatureTracker_KL(const mrpt::containers::yaml& extraParams)
		: CGenericFeatureTracker(extraParams)
	{
	}

   protected:
	void trackFeatures_impl(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointList& inout_featureList) override;
	void trackFeatures_impl(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		TKeyPointfList& inout_featureList) override;

   private:
	template <typename FEATLIST>
	void trackFeatures_impl_templ(
		const mrpt::img::CImage& old_img, const mrpt::img::CImage& new_img,
		FEATLIST& inout_featureList);
};

/**  @}  */	 // end of grouping
}  // namespace mrpt::vision
