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

#include "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/CAtan2LookUpTable.h>

#include <cmath>
#include <vector>

using namespace mrpt::math;

CAtan2LookUpTable::CAtan2LookUpTable() noexcept { this->resize(-1.0, 1.0, -1.0, 1.0, 0.5); }
CAtan2LookUpTable::CAtan2LookUpTable(
    double xmin, double xmax, double ymin, double ymax, double resolution) noexcept
{
  this->resize(xmin, xmax, ymin, ymax, resolution);
}

void CAtan2LookUpTable::resize(
    double xmin, double xmax, double ymin, double ymax, double resolution) noexcept
{
  const double def = .0;
  if (m_grid.getResolution() == resolution)
    m_grid.resize(xmin, xmax, ymin, ymax, def, .0);
  else
    m_grid.setSize(xmin, xmax, ymin, ymax, resolution, &def);

  const size_t nx = m_grid.getSizeX(), ny = m_grid.getSizeY();

  std::vector<double> idx2x(nx), idx2y(ny);

  for (size_t ix = 0; ix < nx; ix++) idx2x[ix] = m_grid.idx2x(ix);
  for (size_t iy = 0; iy < ny; iy++) idx2y[iy] = m_grid.idx2y(iy);

  for (size_t ix = 0; ix < nx; ix++)
  {
    const double x = idx2x[ix];
    for (size_t iy = 0; iy < ny; iy++)
    {
      const double y = idx2y[iy];
      double* cp = m_grid.cellByIndex(ix, iy);
      if (cp) *cp = ::atan2(y, x);
    }
  }
}

CAtan2LookUpTableMultiRes::CAtan2LookUpTableMultiRes(
    const std::map<double, double>& lst_resolutions2extensions) noexcept
{
  resize(lst_resolutions2extensions);
}
void CAtan2LookUpTableMultiRes::resize(
    const std::map<double, double>& lst_resolutions2extensions) noexcept
{
  m_grids.clear();

  for (const auto& it : lst_resolutions2extensions)
  {
    const double res = it.first;
    const double exten = it.second;

    mrpt::containers::CDynamicGrid<double>& dg = m_grids[exten] =
        mrpt::containers::CDynamicGrid<double>(-exten, exten, -exten, exten, res);

    const size_t nx = dg.getSizeX(), ny = dg.getSizeY();
    std::vector<double> idx2x(nx), idx2y(ny);

    for (size_t ix = 0; ix < nx; ix++) idx2x[ix] = dg.idx2x(ix);
    for (size_t iy = 0; iy < ny; iy++) idx2y[iy] = dg.idx2y(iy);

    for (size_t ix = 0; ix < nx; ix++)
    {
      const double x = idx2x[ix];
      for (size_t iy = 0; iy < ny; iy++)
      {
        const double y = idx2y[iy];
        double* cp = dg.cellByIndex(ix, iy);
        if (cp) *cp = ::atan2(y, x);
      }
    }
  }
}

bool CAtan2LookUpTableMultiRes::atan2(double y, double x, double& out_atan2) const noexcept
{
  for (const auto& it : m_grids)
  {
    const double exten = it.first;
    const mrpt::containers::CDynamicGrid<double>& dg = it.second;

    if (std::abs(x) > exten || std::abs(y) > exten) continue;

    const double* cp = dg.cellByPos(x, y);
    if (!cp) continue;
    out_atan2 = *cp;
    return true;
  }
  return false;
}
