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

#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
