/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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 <cmath>  // std::abs
#include <cstdlib>
#include <limits>
#include <optional>

namespace mrpt::containers
{
/** \addtogroup mrpt_containers_find_closest Find closest utility functions
 *  \ingroup mrpt_containers_grp
 * @{ */

/** For an associate container `Container` mapping real number keys to `T`
 * values, searchs for the closest key within a given tolerance, that is,
 * the key closest to `x` within the interval [x-tolerace, x+tolerance].
 * An empty `std::optional` is returned if none is found.
 *
 * Computational cost: `O(log(N)+M)` with `N` the total size of the container,
 * `M` the worst-case number of items in any interval of width `2*tolerance`.
 *
 * \sa find_closest()
 * \note (New in MRPT 2.5.0)
 */
template <typename Container>
std::optional<
	std::pair<typename Container::key_type, typename Container::mapped_type>>
	find_closest_with_tolerance(
		const Container& data, const double x, double tolerance)
{
	const auto t_min = x - tolerance;
	const auto t_max = x + tolerance;

	auto it_lo = data.lower_bound(t_min);
	auto it_hi = data.upper_bound(t_max);

	double min_distance = std::numeric_limits<double>::max();
	std::optional<std::pair<
		typename Container::key_type, typename Container::mapped_type>>
		best;

	for (auto it = it_lo; it != it_hi; ++it)
	{
		if (it == data.end()) continue;
		const auto dist = std::abs(it->first - x);
		if (dist < min_distance)
		{
			min_distance = dist;
			best = {it->first, it->second};
		}
	}

	return best;
}

/** For an associate container `Container` mapping real number keys to `T`
 * values, searchs for the closest key within a given tolerance, that is,
 * the key closest to `x` within the interval [x-tolerace, x+tolerance].
 *
 * An empty `std::optional` is returned if none is found, i.e. if the container
 * was empty.
 *
 * Computational cost: `O(log(N))` with `N` the total size of the container.
 *
 * \sa find_closest_with_tolerance()
 * \note (New in MRPT 2.5.0)
 */
template <typename Container>
std::optional<
	std::pair<typename Container::key_type, typename Container::mapped_type>>
	find_closest(const Container& data, const double x)
{
	typename Container::const_iterator its[2] = {
		data.lower_bound(x), data.upper_bound(x)};

	if (!data.empty() && its[0] != data.begin()) --its[0];

	double min_distance = std::numeric_limits<double>::max();
	std::optional<std::pair<
		typename Container::key_type, typename Container::mapped_type>>
		best;

	for (const auto& it : its)
	{
		if (it == data.end()) continue;
		const auto dist = std::abs(it->first - x);
		if (dist < min_distance)
		{
			min_distance = dist;
			best = {it->first, it->second};
		}
	}

	return best;
}
/**  @} */

}  // namespace mrpt::containers
