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

#include "vision-precomp.h"  // Precompiled headers
//
#include <mrpt/vision/CFeatureExtraction.h>

// Universal include for all versions of OpenCV
#include <mrpt/3rdparty/do_opencv_includes.h>

using namespace mrpt;
using namespace mrpt::img;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace std;

/************************************************************************************************
 *							extractFeaturesKLT
 ************************************************************************************************/
void CFeatureExtraction::extractFeaturesKLT(
    const mrpt::img::CImage& inImg,
    CFeatureList& feats,
    unsigned int init_ID,
    unsigned int nDesiredFeatures,
    const TImageROI& ROI)
{
  MRPT_START
  CTimeLoggerEntry tle(profiler, "extractFeaturesKLT");

#if MRPT_HAS_OPENCV
  const unsigned int MAX_COUNT = 300;

  // -----------------------------------------------------------------
  // Create OpenCV Local Variables
  // -----------------------------------------------------------------
  profiler.enter("extractFeaturesKLT.img2gray");

  const CImage inImg_gray(inImg, FAST_REF_OR_CONVERT_TO_GRAY);
  const cv::Mat& cGrey = inImg_gray.asCvMatRef();

  profiler.leave("extractFeaturesKLT.img2gray");

  const auto nPts = (nDesiredFeatures <= 0) ? MAX_COUNT : nDesiredFeatures;

  // -----------------------------------------------------------------
  // Select good features with subpixel accuracy (USING HARRIS OR KLT)
  // -----------------------------------------------------------------
  const bool use_harris = (options.featsType == featHarris);

  std::vector<cv::Point2f> points;
  profiler.enter("extractFeaturesKLT.goodFeaturesToTrack");

  cv::goodFeaturesToTrack(
      cGrey, points, nPts,
      (double)options.harrisOptions.threshold,  // for rejecting weak local
      // maxima ( with min_eig <
      // threshold*max(eig_image) )
      (double)options.harrisOptions.min_distance,  // minimum distance between features
      cv::noArray(),                               // mask
      3,                                           // blocksize
      use_harris,                                  /* harris */
      options.harrisOptions.k);

  profiler.leave("extractFeaturesKLT.goodFeaturesToTrack");

  const unsigned int count = points.size();

  if (nDesiredFeatures > 0 && count < nPts)
    cout << "\n[WARNING][selectGoodFeaturesKLT]: Only " << count << " of " << nDesiredFeatures
         << " points could be extracted in the image." << endl;

  if (options.FIND_SUBPIXEL && !points.empty())
  {
    profiler.enter("extractFeaturesKLT.cornerSubPix");
    // Subpixel interpolation
    cv::cornerSubPix(
        cGrey, points, cv::Size(3, 3), cv::Size(-1, -1),
        cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.05));

    profiler.leave("extractFeaturesKLT.cornerSubPix");
  }

  CTimeLoggerEntry tle2(profiler, "extractFeaturesKLT.fillFeatsStruct");

  feats.clear();
  unsigned int borderFeats = 0;
  unsigned int nCFeats = init_ID;
  int i = 0;
  const int limit = min(nPts, count);
  int offset = (int)this->options.patchSize / 2 + 1;
  unsigned int imgH = inImg.getHeight();
  unsigned int imgW = inImg.getWidth();

  const float W = options.patchSize * 0.5f;

  for (; i < limit; i++)
  {
    const int xBorderInf = (int)floor(points[i].x - W);
    const int xBorderSup = (int)floor(points[i].x + W);
    const int yBorderInf = (int)floor(points[i].y - W);
    const int yBorderSup = (int)floor(points[i].y + W);

    if (options.patchSize != 0 && ((xBorderSup >= (int)imgW) || (xBorderInf < 0) ||
                                   (yBorderSup >= (int)imgH) || (yBorderInf < 0)))
    {
      borderFeats++;
      continue;
    }

    CFeature ft;

    ft.type = featKLT;
    ft.keypoint.pt.x = points[i].x;    // X position
    ft.keypoint.pt.y = points[i].y;    // Y position
    ft.track_status = status_TRACKED;  // Feature Status
    ft.response = 0.0;                 // A value proportional to the quality of the
    // feature (unused yet)
    ft.keypoint.ID = nCFeats++;        // Feature ID into extraction
    ft.patchSize = options.patchSize;  // The size of the feature patch

    if (options.patchSize > 0)
    {
      ft.patch.emplace();
      inImg.extract_patch(
          *ft.patch, round(ft.keypoint.pt.x) - offset, round(ft.keypoint.pt.y) - offset,
          options.patchSize,
          options.patchSize);  // Image patch surronding the feature
    }

    feats.emplace_back(std::move(ft));

  }  // end while

#else
  THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif

  MRPT_END

}  // end of function
