/* +------------------------------------------------------------------------+
   |                     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 "nav-precomp.h"  // Precomp header
//
#include <mrpt/core/round.h>
#include <mrpt/nav/holonomic/ClearanceDiagram.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

#include <limits>

using namespace mrpt::nav;

ClearanceDiagram::ClearanceDiagram()

    = default;

void ClearanceDiagram::renderAs3DObject(
    mrpt::opengl::CMesh& mesh,
    double min_x,
    double max_x,
    double min_y,
    double max_y,
    double cell_res,
    bool integrate_over_path) const
{
  ASSERT_(cell_res > 0.0);
  ASSERT_(max_x > min_x);
  ASSERT_(max_y > min_y);

  mesh.setXBounds(min_x, max_x);
  mesh.setYBounds(min_y, max_y);
  const int nX = (int)::ceil((max_x - min_x) / cell_res);
  const int nY = (int)::ceil((max_y - min_y) / cell_res);
  const double dx = (max_x - min_x) / nX;
  const double dy = (max_y - min_y) / nY;

  mrpt::math::CMatrixFloat Z(nX, nY);

  if (m_raw_clearances.empty()) return;  // Nothing to do: empty structure!

  for (int iX = 0; iX < nX; iX++)
  {
    const double x = min_x + dx * (0.5 + iX);
    for (int iY = 0; iY < nY; iY++)
    {
      const double y = min_y + dy * (0.5 + iY);

      double clear_val = .0;
      if (x != 0 || y != 0)
      {
        const double alpha = ::atan2(y, x);
        const uint16_t actual_k =
            CParameterizedTrajectoryGenerator::Alpha2index(alpha, m_actual_num_paths);
        const double dist = std::hypot(x, y);
        clear_val = this->getClearance(actual_k, dist, integrate_over_path);
      }
      Z(iX, iY) = clear_val;
    }
  }

  mesh.setZ(Z);
  mesh.enableColorFromZ(true);
  mesh.enableTransparency(true);
  mesh.setColorA_u8(0x50);
  mesh.enableWireFrame(false);
}

void mrpt::nav::ClearanceDiagram::readFromStream(mrpt::serialization::CArchive& in)
{
  uint8_t version;
  in >> version;
  switch (version)
  {
    case 0:
      uint32_t decim_num;
      in.ReadAsAndCastTo<uint32_t, size_t>(m_actual_num_paths);
      in >> decim_num;
      this->resize(m_actual_num_paths, decim_num);
      in >> m_raw_clearances;
      break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

void mrpt::nav::ClearanceDiagram::writeToStream(mrpt::serialization::CArchive& out) const
{
  const uint8_t version = 0;
  out << version;

  out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
  out << m_raw_clearances;
}

ClearanceDiagram::dist2clearance_t& ClearanceDiagram::get_path_clearance(size_t actual_k)
{
  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
}

const ClearanceDiagram::dist2clearance_t& ClearanceDiagram::get_path_clearance(
    size_t actual_k) const
{
  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
}

size_t mrpt::nav::ClearanceDiagram::real_k_to_decimated_k(size_t k) const
{
  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
  const size_t ret = mrpt::round(k * m_k_a2d);
  ASSERT_(ret < m_raw_clearances.size());
  return ret;
}

size_t mrpt::nav::ClearanceDiagram::decimated_k_to_real_k(size_t k) const
{
  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
  const size_t ret = mrpt::round(k * m_k_d2a);
  ASSERT_(ret < m_actual_num_paths);
  return ret;
}

double ClearanceDiagram::getClearance(
    uint16_t actual_k, double dist, bool integrate_over_path) const
{
  if (this->empty())  // If we are not using clearance values, just return a
    // fixed value:
    return 0.0;

  ASSERT_LT_(actual_k, m_actual_num_paths);

  const size_t k = real_k_to_decimated_k(actual_k);

  const auto& rc_k = m_raw_clearances[k];

  double res = 0;
  int avr_count = 0;  // weighted avrg: closer to query points weight more
  // than at path start.
  for (const auto& e : rc_k)
  {
    if (integrate_over_path)
    {
      res = e.second;
      avr_count = 1;
    }
    else
    {
      res += e.second;
      avr_count++;
    }
    if (e.first > dist) break;  // target dist reached.
  }

  if (!avr_count)
  {
    res = rc_k.begin()->second;
  }
  else
  {
    res = res / avr_count;
  }
  return res;
}

void ClearanceDiagram::clear()
{
  m_actual_num_paths = 0;
  m_raw_clearances.clear();
  m_k_a2d = m_k_d2a = .0;
}

void mrpt::nav::ClearanceDiagram::resize(size_t actual_num_paths, size_t decimated_num_paths)
{
  if (decimated_num_paths == 0)
  {
    this->clear();
    return;
  }
  ASSERT_GE_(actual_num_paths, decimated_num_paths);

  m_actual_num_paths = actual_num_paths;
  m_raw_clearances.resize(decimated_num_paths);

  m_k_d2a = double(m_actual_num_paths - 1) / (m_raw_clearances.size() - 1);
  m_k_a2d = double(m_raw_clearances.size() - 1) / (m_actual_num_paths - 1);
}
