/* +------------------------------------------------------------------------+
   |                     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 "common.h"

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

const unsigned int NFEATS = 100;

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

// ------------------------------------------------------
//				Benchmark: Harris + CC
// ------------------------------------------------------
double feature_matching_test_Harris_CC(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsHarris_L, featsHarris_R;
  CMatchedFeatureList mHarris;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featHarris;

  // size_t				nMatches;
  TMatchingOptions opt;
  const size_t N = 20;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsHarris_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsHarris_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsHarris_L, featsHarris_R, mHarris);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsHarris_L.size() << " R: " <<
  // featsHarris_R.size() << " M: " << mHarris.size() << endl;

  return T;
}

// ------------------------------------------------------
//				Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_Harris_SAD(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsHarris_L, featsHarris_R;
  CMatchedFeatureList mHarris;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featHarris;

  TMatchingOptions opt;
  const size_t N = 20;
  opt.matching_method = TMatchingOptions::mmSAD;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsHarris_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsHarris_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsHarris_L, featsHarris_R, mHarris, opt);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsHarris_L.size() << " R: " <<
  // featsHarris_R.size() << " M: " << mHarris.size() << endl;

  return T;
}

// ------------------------------------------------------
//				Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_SIFT(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsSIFT_L, featsSIFT_R;
  CMatchedFeatureList mSIFT;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featSIFT;

  // size_t				nMatches;
  TMatchingOptions opt;
  const size_t N = 5;
  opt.matching_method = TMatchingOptions::mmDescriptorSIFT;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsSIFT_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsSIFT_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsSIFT_L, featsSIFT_R, mSIFT, opt);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsSIFT_L.size() << " R: " <<
  // featsSIFT_R.size() << " M: " << mSIFT.size() << endl;

  return T;
}

// ------------------------------------------------------
//				Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_SURF(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsSURF_L, featsSURF_R;
  CMatchedFeatureList mSURF;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featSURF;

  // size_t				nMatches;
  TMatchingOptions opt;
  const size_t N = 10;
  opt.matching_method = TMatchingOptions::mmDescriptorSURF;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsSURF_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsSURF_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsSURF_L, featsSURF_R, mSURF, opt);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsSURF_L.size() << " R: " <<
  // featsSURF_R.size() << " M: " << mSURF.size() << endl;

  return T;
}

// ------------------------------------------------------
//				Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_FAST_CC(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsFAST_L, featsFAST_R;
  CMatchedFeatureList mFAST;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featFAST;

  // size_t				nMatches;
  TMatchingOptions opt;
  const size_t N = 20;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsFAST_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsFAST_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsFAST_L, featsFAST_R, mFAST, opt);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsFAST_L.size() << " R: " <<
  // featsFAST_R.size() << " M: " << mFAST.size() << endl;

  return T;
}

// ------------------------------------------------------
//				Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_FAST_SAD(int w, int h)
{
  CTicTac tictac;

  CImage imL, imR;
  CFeatureExtraction fExt;
  CFeatureList featsFAST_L, featsFAST_R;
  CMatchedFeatureList mFAST;

  getTestImage(0, imR);
  getTestImage(1, imL);

  // Extract features: HARRIS
  fExt.options.featsType = featFAST;

  // size_t				nMatches;
  TMatchingOptions opt;
  const size_t N = 20;
  opt.matching_method = TMatchingOptions::mmSAD;

  // HARRIS
  tictac.Tic();
  for (size_t i = 0; i < N; i++)
  {
    fExt.detectFeatures(imL, featsFAST_L, 0, NFEATS);
    fExt.detectFeatures(imR, featsFAST_R, 0, NFEATS);
    // nMatches =
    matchFeatures(featsFAST_L, featsFAST_R, mFAST, opt);
  }
  const double T = tictac.Tac() / N;

  //	cout << endl << "L: " << featsFAST_L.size() << " R: " <<
  // featsFAST_R.size() << " M: " << mFAST.size() << endl;

  return T;
}

// ------------------------------------------------------
// register_tests_feature_extraction
// ------------------------------------------------------
void register_tests_feature_matching()
{
  lstTests.emplace_back(
      "feature_matching [640x480]: Harris + CC", feature_matching_test_Harris_CC, 640, 480);
  lstTests.emplace_back(
      "feature_matching [640x480]: Harris + SAD", feature_matching_test_Harris_SAD, 640, 480);
#if 0  // crash in opencv 3.1?
	lstTests.push_back( TestData("feature_matching [640x480]: SIFT", feature_matching_test_SIFT, 640, 480 ) );
#endif
  lstTests.emplace_back("feature_matching [640x480]: SURF", feature_matching_test_SURF, 640, 480);
  lstTests.emplace_back(
      "feature_matching [640x480]: FAST + CC", feature_matching_test_FAST_CC, 640, 480);
  lstTests.emplace_back(
      "feature_matching [640x480]: FAST + SAD", feature_matching_test_FAST_SAD, 640, 480);
}
