/* +------------------------------------------------------------------------+
   |                     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/math/geometry.h>	 // distance()

namespace mrpt::math
{
/**  Auxiliary functor class to compute polygon's center */
template <class T, int N>
class FAddPoint
{
   public:
	T& object;
	FAddPoint(T& o) : object(o)
	{
		for (size_t i = 0; i < N; i++)
			object[i] = 0.0;
	}
	void operator()(const T& o)
	{
		for (size_t i = 0; i < N; i++)
			object[i] += o[i];
	}
};

template <class T>
inline void removeUnusedVertices(T& poly)
{
	size_t N = poly.size();
	if (N < 3) return;
	std::vector<size_t> unused;
	if (std::abs(
			mrpt::math::distance(poly[N - 1], poly[0]) +
			mrpt::math::distance(poly[0], poly[1]) -
			mrpt::math::distance(poly[N - 1], poly[1])) <
		mrpt::math::getEpsilon())
		unused.push_back(0);
	for (size_t i = 1; i < N - 1; i++)
		if (std::abs(
				mrpt::math::distance(poly[i - 1], poly[i]) +
				mrpt::math::distance(poly[i], poly[i + 1]) -
				mrpt::math::distance(poly[i - 1], poly[i + 1])) <
			mrpt::math::getEpsilon())
			unused.push_back(i);
	if (std::abs(
			mrpt::math::distance(poly[N - 2], poly[N - 1]) +
			mrpt::math::distance(poly[N - 1], poly[0]) -
			mrpt::math::distance(poly[N - 2], poly[0])) <
		mrpt::math::getEpsilon())
		unused.push_back(N - 1);
	unused.push_back(N);
	size_t diff = 1;
	for (size_t i = 0; i < unused.size() - 1; i++)
	{
		size_t last = unused[i + 1];
		for (size_t j = unused[i] + 1 - diff; j < last - diff; j++)
			poly[j] = poly[j + diff];
	}
	poly.resize(N + 1 - unused.size());
}
template <class T>
inline void removeRepVertices(T& poly)
{
	size_t N = poly.size();
	if (N < 3) return;
	std::vector<size_t> rep;
	for (size_t i = 0; i < N - 1; i++)
		if (mrpt::math::distance(poly[i], poly[i + 1]) < getEpsilon())
			rep.push_back(i);
	if (mrpt::math::distance(poly[N - 1], poly[0]) < getEpsilon())
		rep.push_back(N - 1);
	rep.push_back(N);
	size_t diff = 1;
	for (size_t i = 0; i < rep.size() - 1; i++)
	{
		size_t last = rep[i + 1];
		for (size_t j = rep[i] + 1 - diff; j < last - diff; j++)
			poly[j] = poly[j + diff];
	}
	poly.resize(N + 1 - rep.size());
}

}  // namespace mrpt::math
