/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#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);
}
