/* +------------------------------------------------------------------------+
   |                     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 <mrpt/img/CImage.h>
#include <mrpt/vision/CFeatureExtraction.h>

#include <iomanip>

#include "common.h"

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

extern void getTestImage(unsigned int img_index, mrpt::img::CImage& out_img);

template <TKeyPointMethod FEAT_TYPE>
double benchmark_detectFeatures(int N, [[maybe_unused]] int h)
{
  // Generate a random image
  CImage img;
  getTestImage(0, img);
  CFeatureExtraction fExt;
  fExt.profiler.enable();
  fExt.options.featsType = FEAT_TYPE;
  for (int i = 0; i < N; i++)
  {
    CFeatureList fs;
    fExt.detectFeatures(img, fs);
    if (i == (N - 1)) std::cout << "(" << std::setw(4) << fs.size() << " found)\n";
  }
  return fExt.profiler.getMeanTime("detectFeatures");
}

// ------------------------------------------------------
//				Benchmark: descriptor
// ------------------------------------------------------
template <TDescriptorType DESCRIPTOR_TYPE>
double benchmark_computeDescriptor(int N, int num_feats)
{
  CImage img;
  getTestImage(0, img);

  CFeatureExtraction fExt;
  fExt.profiler.enable();
  fExt.options.featsType = featFAST;

  for (int i = 0; i < N; i++)
  {
    CFeatureList fs;
    fExt.detectFeatures(img, fs, 0 /*id*/, num_feats);
    fExt.computeDescriptors(img, fs, DESCRIPTOR_TYPE);
  }

  return fExt.profiler.getMeanTime("computeDescriptors");
}

// ------------------------------------------------------
//				Benchmark: FASTER
// ------------------------------------------------------
template <mrpt::vision::TKeyPointMethod TYP, int MAX_N_FEATS>
double benchmark_detectFeatures_FASTER(int N, int threshold)
{
  CTicTac tictac;

  // Generate a random image
  CImage img;
  getTestImage(0, img);

  CFeatureExtraction fExt;
  CFeatureList feats;

  fExt.options.featsType = TYP;
  fExt.options.FASTOptions.threshold = threshold;
  fExt.options.patchSize = 0;

  img = img.grayscale();

  tictac.Tic();
  for (int i = 0; i < N; i++) fExt.detectFeatures(img, feats, 0, MAX_N_FEATS);

  const double T = tictac.Tac() / N;
  return T;
}

// ------------------------------------------------------
// register_tests_feature_extraction
// ------------------------------------------------------
void register_tests_feature_extraction()
{
  // Detectors:
  lstTests.emplace_back(
      "feature_extraction [640x480]: Harris (OpenCV)", benchmark_detectFeatures<featHarris>, 30);
  lstTests.emplace_back(
      "feature_extraction [640x480]: KLT (OpenCV)", benchmark_detectFeatures<featKLT>, 30);
  lstTests.emplace_back(
      "feature_extraction [640x480]: SIFT detect (OpenCV)", benchmark_detectFeatures<featSIFT>, 5);
  lstTests.emplace_back(
      "feature_extraction [640x480]: SURF (OpenCV)", benchmark_detectFeatures<featSURF>, 10);
  lstTests.emplace_back(
      "feature_extraction [640x480]: ORB (OpenCV)", benchmark_detectFeatures<featORB>, 10);
  lstTests.emplace_back(
      "feature_extraction [640x480]: FAST (OpenCV)", benchmark_detectFeatures<featFAST>, 100);

  lstTests.emplace_back(
      "feature_extraction [640x480]: AKAZE (OpenCV)", benchmark_detectFeatures<featAKAZE>, 5);
  lstTests.emplace_back(
      "feature_extraction [640x480]: LSD (OpenCV)", benchmark_detectFeatures<featLSD>, 5);

  // Descriptors:
  lstTests.emplace_back(
      "feature_computeDescriptor [640x480,N=100]: ORB (OpenCV)",
      benchmark_computeDescriptor<descORB>, 30, 100);
  lstTests.emplace_back(
      "feature_computeDescriptor [640x480,N=100]: Spin (OpenCV)",
      benchmark_computeDescriptor<descSpinImages>, 30, 100);
  lstTests.emplace_back(
      "feature_computeDescriptor [640x480,N=100]: SURF (OpenCV)",
      benchmark_computeDescriptor<descSURF>, 6, 100);
  /*	lstTests.emplace_back(
      "feature_computeDescriptor [640x480,N=100]: SIFT (OpenCV)",
      benchmark_computeDescriptor<descSIFT>, 6, 100);
      */
}
