/* +------------------------------------------------------------------------+
   |                     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/vision/types.h>

namespace mrpt::vision
{
/** \addtogroup  mrptvision_features
	@{ */

/** Search for pairings between two sets of visual descriptors (for now, only
 * SURF
 *   and SIFT features are considered).
 *  Pairings are returned in one of two formats (or both of them
 * simultaneously),
 *   depending on the non-NULL output containers present in the call.
 *
 * \code
 *  CFeatureList  feats1, feats2;
 *  // Populate feature lists [...]
 *
 *  // Create kd-tree for SIFT features of "feats2":
 *  TSIFTDescriptorsKDTreeIndex<double>  feats2_kdtree(feats2);
 *
 *  // Search correspondences:
 *  std::vector<std::vector<size_t>>             pairings_1_to_multi_2;
 *  std::vector<std::pair<size_t,size_t> > pairings_1_to_2;
 *  mrpt::vision::find_descriptor_pairings(
 *     &pairings_1_to_multi_2,   // Can be set to nullptr if not needed
 *     &pairings_1_to_2,         // Can be set to nullptr if not needed
 *     feats1, feats2_kdtree,    // The two sets of features
 *     mrpt::vision::descSIFT    // Select descriptor to use
 *     // [further optional params]
 *     );
 * \endcode
 *
 * \sa TSIFTDescriptorsKDTreeIndex, TSURFDescriptorsKDTreeIndex
 */
template <class DESCRIPTOR_KDTREE>
size_t find_descriptor_pairings(
	std::vector<std::vector<size_t>>* pairings_1_to_multi_2,
	std::vector<std::pair<size_t, size_t>>* pairings_1_to_2,
	const CFeatureList& feats_img1, const DESCRIPTOR_KDTREE& feats_img2_kdtree,
	const mrpt::vision::TDescriptorType descriptor = descSIFT,
	const size_t max_neighbors = 4, const double max_relative_distance = 1.2,
	const typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType max_distance =
		std::numeric_limits<
			typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType>::max())
{
	MRPT_START
	ASSERT_GE_(max_neighbors, 1);
	ASSERT_(pairings_1_to_multi_2 != nullptr || pairings_1_to_2 != nullptr);

	/** The expected data type of elements for the kd-tree*/
	using KDTreeElementType = typename DESCRIPTOR_KDTREE::kdtree_t::ElementType;
	using KDTreeDistanceType =
		typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType;

	const size_t N = feats_img1.size();
	if (pairings_1_to_multi_2)
		pairings_1_to_multi_2->assign(
			N, std::vector<size_t>());	// Reset output container
	if (pairings_1_to_2)
	{
		pairings_1_to_2->clear();
		pairings_1_to_2->reserve(N);
	}

	size_t overall_pairs = 0;

	if (!N) return overall_pairs;  // No features -> nothing to do

	if (descriptor == descSIFT)
	{
		ASSERTMSG_(
			feats_img1[0].descriptors.hasDescriptorSIFT(),
			"Request to match SIFT features but feats_img1 has no SIFT "
			"descriptors!");
		ASSERTMSG_(
			sizeof(KDTreeElementType) ==
				sizeof((*feats_img1[0].descriptors.SIFT)[0]),
			"Incorrect data type kd_tree::ElementType for SIFT (should be "
			"uint8_t)");
	}
	else if (descriptor == descSURF)
	{
		ASSERTMSG_(
			feats_img1[0].descriptors.hasDescriptorSURF(),
			"Request to match SURF features but feats_img1 has no SURF "
			"descriptors!");
		ASSERTMSG_(
			sizeof(KDTreeElementType) ==
				sizeof((*feats_img1[0].descriptors.SURF)[0]),
			"Incorrect data type kd_tree::ElementType for SURF (should be "
			"float)");
	}
	else
	{
		THROW_EXCEPTION(
			"This function only supports SIFT or SURFT descriptors");
	}

	std::vector<size_t> indices(max_neighbors);
	std::vector<double> distances(max_neighbors);

	for (size_t i = 0; i < N; i++)
	{
		const CFeature::TDescriptors& descs = feats_img1[i].descriptors;

		const void* ptr_query;
		if (descriptor == descSIFT) ptr_query = &(*descs.SIFT)[0];
		else if (descriptor == descSURF)
			ptr_query = &(*descs.SURF)[0];

		feats_img2_kdtree.get_kdtree().knnSearch(
			static_cast<const KDTreeElementType*>(ptr_query),  // Query point
			max_neighbors,	// Number of neigbors
			&indices[0], &distances[0]	// Output
		);

		// Include all correspondences below the absolute and the relative
		// threshold (indices comes ordered by distances):
		const KDTreeDistanceType this_thresh =
			std::min(max_relative_distance * distances[0], max_distance);
		for (size_t j = 0; j < max_neighbors; j++)
		{
			if (distances[j] <= this_thresh)
			{
				overall_pairs++;
				if (pairings_1_to_multi_2)
					(*pairings_1_to_multi_2)[i].push_back(indices[j]);
				if (pairings_1_to_2)
					pairings_1_to_2->push_back(std::make_pair(i, indices[j]));
			}
			else
				break;
		}
	}
	return overall_pairs;
	MRPT_END
}

/** @} */
}  // namespace mrpt::vision
