/* +------------------------------------------------------------------------+
   |                     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/WorkerThreadsPool.h>
#include <mrpt/core/get_env.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <mrpt/math/geometry.h>
#include <mrpt/nav/tpspace/CPTG_DiffDrive_CollisionGridBased.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/CTicTac.h>

#include <iostream>

using namespace mrpt::nav;

/** Constructor: possible values in "params":
 *   - ref_distance: The maximum distance in PTGs
 *   - resolution: The cell size
 *   - v_max, w_max: Maximum robot speeds.
 */
CPTG_DiffDrive_CollisionGridBased::CPTG_DiffDrive_CollisionGridBased() :
    m_collisionGrid(-1, 1, -1, 1, 0.5, this)
{
}

void CPTG_DiffDrive_CollisionGridBased::loadDefaultParams()
{
  CParameterizedTrajectoryGenerator::loadDefaultParams();
  CPTG_RobotShape_Polygonal::loadDefaultParams();

  m_resolution = 0.10;
  V_MAX = 1.0;
  W_MAX = mrpt::DEG2RAD(120);
}

void CPTG_DiffDrive_CollisionGridBased::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
{
  CParameterizedTrajectoryGenerator::loadFromConfigFile(cfg, sSection);
  CPTG_RobotShape_Polygonal::loadShapeFromConfigFile(cfg, sSection);

  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(resolution, double, m_resolution, cfg, sSection);
  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(v_max_mps, double, V_MAX, cfg, sSection);
  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(w_max_dps, double, W_MAX, cfg, sSection);
  MRPT_LOAD_CONFIG_VAR(turningRadiusReference, double, cfg, sSection);
}
void CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(
    mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
  MRPT_START
  const int WN = 25, WV = 30;

  CParameterizedTrajectoryGenerator::saveToConfigFile(cfg, sSection);

  cfg.write(
      sSection, "resolution", m_resolution, WN, WV,
      "Resolution of the collision-check look-up-table [m].");
  cfg.write(
      sSection, "v_max_mps", V_MAX, WN, WV, "Maximum linear velocity for trajectories [m/s].");
  cfg.write(
      sSection, "w_max_dps", mrpt::RAD2DEG(W_MAX), WN, WV,
      "Maximum angular velocity for trajectories [deg/s].");
  cfg.write(
      sSection, "turningRadiusReference", turningRadiusReference, WN, WV,
      "An approximate dimension of the robot (not a critical parameter) "
      "[m].");

  CPTG_RobotShape_Polygonal::saveToConfigFile(cfg, sSection);

  MRPT_END
}

mrpt::serialization::CArchive& mrpt::nav::operator<<(
    mrpt::serialization::CArchive& o, const mrpt::nav::TCPoint& p)
{
  o << p.x << p.y << p.phi << p.t << p.dist << p.v << p.w;
  return o;
}
mrpt::serialization::CArchive& mrpt::nav::operator>>(
    mrpt::serialization::CArchive& i, mrpt::nav::TCPoint& p)
{
  i >> p.x >> p.y >> p.phi >> p.t >> p.dist >> p.v >> p.w;
  return i;
}

/*---------------------------------------------------------------
          simulateTrajectories
  Solve trajectories and fill cells.
  ---------------------------------------------------------------*/
void CPTG_DiffDrive_CollisionGridBased::simulateTrajectories(
    float max_time,
    float max_dist,
    unsigned int max_n,
    float diferencial_t,
    float min_dist,
    float* out_max_acc_v,
    float* out_max_acc_w)
{
  using mrpt::square;

  internal_deinitialize();  // Free previous paths

  m_stepTimeDuration = diferencial_t;

  // Reserve the size in the buffers:
  m_trajectory.resize(m_alphaValuesCount);

  const float radio_max_robot = 1.0f;  // Arbitrary "robot radius", only to
  // determine the spacing of points
  // under pure rotation

  // Aux buffer:
  TCPointVector points;

  float ult_dist, ult_dist1, ult_dist2;

  // For the grid:
  float x_min = 1e3f, x_max = -1e3;
  float y_min = 1e3f, y_max = -1e3;

  // Para averiguar las maximas ACELERACIONES lineales y angulares:
  float max_acc_lin, max_acc_ang;

  max_acc_lin = max_acc_ang = 0;

  try
  {
    for (unsigned int k = 0; k < m_alphaValuesCount; k++)
    {
      // Simulate / evaluate the trajectory selected by this "alpha":
      // ------------------------------------------------------------
      const float alpha = index2alpha(k);

      points.clear();
      float t = .0f, dist = .0f, girado = .0f;
      float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f, _y = .0f, _phi = .0f;

      // Sliding window with latest movement commands (for the optional
      // low-pass filtering):
      float last_vs[2] = {.0f, .0f}, last_ws[2] = {.0f, .0f};

      // Add the first, initial point:
      points.push_back(TCPoint(x, y, phi, t, dist, v, w));

      // Simulate until...
      while (t < max_time && dist < max_dist && points.size() < max_n && fabs(girado) < 1.95 * M_PI)
      {
        // Max. aceleraciones:
        if (t > 1)
        {
          float acc_lin = fabs((last_vs[0] - last_vs[1]) / diferencial_t);
          float acc_ang = fabs((last_ws[0] - last_ws[1]) / diferencial_t);
          mrpt::keep_max(max_acc_lin, acc_lin);
          mrpt::keep_max(max_acc_ang, acc_ang);
        }

        // Compute new movement command (v,w):
        ptgDiffDriveSteeringFunction(alpha, t, x, y, phi, v, w);

        // History of v/w ----------------------------------
        last_vs[1] = last_vs[0];
        last_ws[1] = last_ws[0];
        last_vs[0] = v;
        last_ws[0] = w;
        // -------------------------------------------

        // Finite difference equation:
        x += cos(phi) * v * diferencial_t;
        y += sin(phi) * v * diferencial_t;
        phi += w * diferencial_t;

        // Counters:
        girado += w * diferencial_t;

        float v_inTPSpace = sqrt(square(v) + square(w * turningRadiusReference));

        dist += v_inTPSpace * diferencial_t;

        t += diferencial_t;

        // Save sample if we moved far enough:
        ult_dist1 = sqrt(square(_x - x) + square(_y - y));
        ult_dist2 = fabs(radio_max_robot * (_phi - phi));
        ult_dist = std::max(ult_dist1, ult_dist2);

        if (ult_dist > min_dist)
        {
          // Set the (v,w) to the last record:
          points.back().v = v;
          points.back().w = w;

          // And add the new record:
          points.push_back(TCPoint(x, y, phi, t, dist, v, w));

          // For the next iter:
          _x = x;
          _y = y;
          _phi = phi;
        }

        // for the grid:
        x_min = std::min(x_min, x);
        x_max = std::max(x_max, x);
        y_min = std::min(y_min, y);
        y_max = std::max(y_max, y);
      }

      // Add the final point:
      points.back().v = v;
      points.back().w = w;
      points.push_back(TCPoint(x, y, phi, t, dist, v, w));

      // Save data to C-Space path structure:
      m_trajectory[k] = points;

    }  // end for "k"

    // Save accelerations
    if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
    if (out_max_acc_w) *out_max_acc_w = max_acc_ang;

    // --------------------------------------------------------
    // Build the speeding-up grid for lambda function:
    // --------------------------------------------------------
    const TCellForLambdaFunction defaultCell;
    m_lambdaFunctionOptimizer.setSize(
        x_min - 0.5f, x_max + 0.5f, y_min - 0.5f, y_max + 0.5f, 0.25f, &defaultCell);

    for (uint16_t k = 0; k < m_alphaValuesCount; k++)
    {
      const auto M = static_cast<uint32_t>(m_trajectory[k].size());
      for (uint32_t n = 0; n < M; n++)
      {
        TCellForLambdaFunction* cell =
            m_lambdaFunctionOptimizer.cellByPos(m_trajectory[k][n].x, m_trajectory[k][n].y);
        ASSERT_(cell);
        // Keep limits:
        mrpt::keep_min(cell->k_min, k);
        mrpt::keep_max(cell->k_max, k);
        mrpt::keep_min(cell->n_min, n);
        mrpt::keep_max(cell->n_max, n);
      }
    }
  }
  catch (...)
  {
    std::cout << "[CPTG_DiffDrive_CollisionGridBased::simulateTrajectories] "
                 "Simulation aborted: unexpected exception!\n";
  }
}

/** In this class, `out_action_cmd` contains: [0]: linear velocity (m/s),  [1]:
 * angular velocity (rad/s) */
mrpt::kinematics::CVehicleVelCmd::Ptr CPTG_DiffDrive_CollisionGridBased::directionToMotionCommand(
    uint16_t k) const
{
  float v, w;
  ptgDiffDriveSteeringFunction(index2alpha(k), 0, 0, 0, 0, v, w);

  auto* cmd = new mrpt::kinematics::CVehicleVelCmd_DiffDriven();
  cmd->lin_vel = v;
  cmd->ang_vel = w;
  return mrpt::kinematics::CVehicleVelCmd::Ptr(cmd);
}

/*---------------------------------------------------------------
          getTPObstacle
  ---------------------------------------------------------------*/
const CPTG_DiffDrive_CollisionGridBased::TCollisionCell&
CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::getTPObstacle(
    const float obsX, const float obsY) const
{
  static const TCollisionCell emptyCell;
  const TCollisionCell* cell = cellByPos(obsX, obsY);
  return cell != nullptr ? *cell : emptyCell;
}

/*---------------------------------------------------------------
  Updates the info into a cell: It updates the cell only
    if the distance d for the path k is lower than the previous value:
  ---------------------------------------------------------------*/
void CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::updateCellInfo(
    const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist)
{
  TCollisionCell* cell = cellByIndex(icx, icy);
  if (!cell) return;

  // For such a small number of elements, brute-force search is not such a bad
  // idea:
  auto itK = cell->end();
  for (auto it = cell->begin(); it != cell->end(); ++it)
    if (it->first == k)
    {
      itK = it;
      break;
    }

  if (itK == cell->end())
  {  // New entry:
    cell->push_back(std::make_pair(k, dist));
  }
  else
  {  // Only update that "k" if the distance is shorter now:
    if (dist < itK->second) itK->second = dist;
  }
}

/*---------------------------------------------------------------
          Save to file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::saveColGridsToFile(
    const std::string& filename, const mrpt::math::CPolygon& computed_robotShape) const
{
  try
  {
    mrpt::io::CFileGZOutputStream fo(filename);
    if (!fo.fileOpenCorrectly()) return false;

    const uint32_t n = 1;  // for backwards compatibility...
    auto arch = mrpt::serialization::archiveFrom(fo);
    arch << n;
    return m_collisionGrid.saveToFile(&arch, computed_robotShape);
  }
  catch (...)
  {
    return false;
  }
}

/*---------------------------------------------------------------
          Load from file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::loadColGridsFromFile(
    const std::string& filename, const mrpt::math::CPolygon& current_robotShape)
{
  try
  {
    mrpt::io::CFileGZInputStream fi(filename);
    if (!fi.fileOpenCorrectly()) return false;
    auto arch = mrpt::serialization::archiveFrom(fi);

    uint32_t n;
    arch >> n;
    if (n != 1) return false;  // Incompatible (old) format, just discard and
    // recompute.

    return m_collisionGrid.loadFromFile(&arch, current_robotShape);
  }
  catch (...)
  {
    return false;
  }
}

const uint32_t COLGRID_FILE_MAGIC = 0xC0C0C0C3;

/*---------------------------------------------------------------
          Save to file
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::saveToFile(
    mrpt::serialization::CArchive* f, const mrpt::math::CPolygon& computed_robotShape) const
{
  try
  {
    if (!f) return false;

    const uint8_t serialize_version = 2;  // v1: As of jun 2012, v2: As of dec-2013

    // Save magic signature && serialization version:
    *f << COLGRID_FILE_MAGIC << serialize_version;

    // Robot shape:
    *f << computed_robotShape;

    // and standard PTG data:
    *f << m_parent->getDescription() << m_parent->getAlphaValuesCount() << d2f(m_parent->getMax_V())
       << d2f(m_parent->getMax_W());

    *f << m_x_min << m_x_max << m_y_min << m_y_max;
    *f << m_resolution;

    // v1 was:  *f << m_map;
    uint32_t N = m_map.size();
    *f << N;
    for (uint32_t i = 0; i < N; i++)
    {
      uint32_t M = m_map[i].size();
      *f << M;
      for (uint32_t k = 0; k < M; k++) *f << m_map[i][k].first << m_map[i][k].second;
    }

    return true;
  }
  catch (...)
  {
    return false;
  }
}

/*---------------------------------------------------------------
            loadFromFile
  ---------------------------------------------------------------*/
bool CPTG_DiffDrive_CollisionGridBased::CCollisionGrid::loadFromFile(
    mrpt::serialization::CArchive* f, const mrpt::math::CPolygon& current_robotShape)
{
  try
  {
    if (!f) return false;

    // Return false if the file contents doesn't match what we expected:
    uint32_t file_magic;
    *f >> file_magic;

    // It doesn't seem to be a valid file or was in an old format, just
    // recompute the grid:
    if (COLGRID_FILE_MAGIC != file_magic) return false;

    uint8_t serialized_version;
    *f >> serialized_version;

    switch (serialized_version)
    {
      case 2:
      {
        mrpt::math::CPolygon stored_shape;
        *f >> stored_shape;

        const bool shapes_match =
            (stored_shape.size() == current_robotShape.size() &&
             std::equal(stored_shape.begin(), stored_shape.end(), current_robotShape.begin()));

        if (!shapes_match) return false;  // Must recompute if the robot shape changed.
      }
      break;

      case 1:
      default:
        // Unknown version: Maybe we are loading a file from a more
        // recent version of MRPT? Whatever, we can't read it: It's
        // safer just to re-generate the PTG data
        return false;
    };

    // Standard PTG data:
    const std::string expected_desc = m_parent->getDescription();
    std::string desc;
    *f >> desc;
    if (desc != expected_desc) return false;

// and standard PTG data:
#define READ_UINT16_CHECK_IT_MATCHES_STORED(_VAR) \
  {                                               \
    uint16_t ff;                                  \
    *f >> ff;                                     \
    if (ff != _VAR) return false;                 \
  }
#define READ_FLOAT_CHECK_IT_MATCHES_STORED(_VAR)   \
  {                                                \
    float ff;                                      \
    *f >> ff;                                      \
    if (std::abs(ff - _VAR) > 1e-4f) return false; \
  }
#define READ_DOUBLE_CHECK_IT_MATCHES_STORED(_VAR) \
  {                                               \
    double ff;                                    \
    *f >> ff;                                     \
    if (std::abs(ff - _VAR) > 1e-6) return false; \
  }

    READ_UINT16_CHECK_IT_MATCHES_STORED(m_parent->getAlphaValuesCount())
    READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_V())
    READ_FLOAT_CHECK_IT_MATCHES_STORED(m_parent->getMax_W())

    // Cell dimensions:
    READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_x_min)
    READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_x_max)
    READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_y_min)
    READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_y_max)
    READ_DOUBLE_CHECK_IT_MATCHES_STORED(m_resolution)

    // OK, all parameters seem to be exactly the same than when we
    // precomputed the table: load it.
    // v1 was:  *f >> m_map;
    uint32_t N;
    *f >> N;
    m_map.resize(N);
    for (uint32_t i = 0; i < N; i++)
    {
      uint32_t M;
      *f >> M;
      m_map[i].resize(M);
      for (uint32_t k = 0; k < M; k++) *f >> m_map[i][k].first >> m_map[i][k].second;
    }

    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CCollisionGrid::loadFromFile] " << e.what();
    return false;
  }
  catch (...)
  {
    return false;
  }
}

bool CPTG_DiffDrive_CollisionGridBased::inverseMap_WS2TP(
    double x, double y, int& out_k, double& out_d, double tolerance_dist) const
{
  using mrpt::square;

  ASSERTMSG_(m_alphaValuesCount > 0, "Have you called simulateTrajectories() first?");

  // -------------------------------------------------------------------
  // Optimization: (24-JAN-2007 @ Jose Luis Blanco):
  //  Use a "grid" to determine the range of [k,d] values to check!!
  //  If the point (x,y) is not found in the grid, then directly skip
  //  to the next step.
  // -------------------------------------------------------------------
  uint16_t k_min = 0, k_max = m_alphaValuesCount - 1;
  uint32_t n_min = 0, n_max = 0;
  bool at_least_one = false;

  // Cell indexes:
  int cx0 = m_lambdaFunctionOptimizer.x2idx(x);
  int cy0 = m_lambdaFunctionOptimizer.y2idx(y);

  // (cx,cy)
  for (int cx = cx0 - 1; cx <= cx0 + 1; cx++)
  {
    for (int cy = cy0 - 1; cy <= cy0 + 1; cy++)
    {
      const TCellForLambdaFunction* cell = m_lambdaFunctionOptimizer.cellByIndex(cx, cy);
      if (cell && !cell->isEmpty())
      {
        if (!at_least_one)
        {
          k_min = cell->k_min;
          k_max = cell->k_max;
          n_min = cell->n_min;
          n_max = cell->n_max;
          at_least_one = true;
        }
        else
        {
          mrpt::keep_min(k_min, cell->k_min);
          mrpt::keep_max(k_max, cell->k_max);

          mrpt::keep_min(n_min, cell->n_min);
          mrpt::keep_max(n_max, cell->n_max);
        }
      }
    }
  }

  // Try to find a closest point to the paths:
  // ----------------------------------------------
  int selected_k = -1;
  float selected_d = 0;
  float selected_dist = std::numeric_limits<float>::max();

  if (at_least_one)  // Otherwise, don't even lose time checking...
  {
    ASSERT_LT_(k_max, m_trajectory.size());
    for (int k = k_min; k <= k_max; k++)
    {
      const size_t n_real = m_trajectory[k].size();
      const uint32_t n_max_this = std::min(static_cast<uint32_t>(n_real ? n_real - 1 : 0), n_max);

      for (uint32_t n = n_min; n <= n_max_this; n++)
      {
        const float dist_a_punto =
            square(m_trajectory[k][n].x - x) + square(m_trajectory[k][n].y - y);
        if (dist_a_punto < selected_dist)
        {
          selected_dist = dist_a_punto;
          selected_k = k;
          selected_d = m_trajectory[k][n].dist;
        }
      }
    }
  }

  if (selected_k != -1)
  {
    out_k = selected_k;
    out_d = selected_d / refDistance;
    return (selected_dist <= square(tolerance_dist));
  }

  // If not found, compute an extrapolation:

  // ------------------------------------------------------------------------------------
  // Given a point (x,y), compute the "k_closest" whose extrapolation
  //  is closest to the point, and the associated "d_closest" distance,
  //  which can be normalized by "1/refDistance" to get TP-Space distances.
  // ------------------------------------------------------------------------------------
  selected_dist = std::numeric_limits<float>::max();
  for (uint16_t k = 0; k < m_alphaValuesCount; k++)
  {
    const int n = int(m_trajectory[k].size()) - 1;
    const float dist_a_punto = square(m_trajectory[k][n].dist) + square(m_trajectory[k][n].x - x) +
                               square(m_trajectory[k][n].y - y);

    if (dist_a_punto < selected_dist)
    {
      selected_dist = dist_a_punto;
      selected_k = k;
      selected_d = dist_a_punto;
    }
  }

  selected_d = std::sqrt(selected_d);

  out_k = selected_k;
  out_d = selected_d / refDistance;

  // If the target dist. > refDistance, then it's normal that we had to
  // extrapolate.
  // Otherwise, it may actually mean that the target is not reachable by this
  // set of paths:
  const float target_dist = std::sqrt(x * x + y * y);
  return (target_dist > refDistance);
}

void CPTG_DiffDrive_CollisionGridBased::setRefDistance(const double refDist)
{
  ASSERTMSG_(
      m_trajectory.empty(),
      "Changing reference distance not allowed in this class after "
      "initialization!");
  this->refDistance = refDist;
}

void CPTG_DiffDrive_CollisionGridBased::internal_processNewRobotShape()
{
  ASSERTMSG_(
      m_trajectory.empty(), "Changing robot shape not allowed in this class after initialization!");
}

void CPTG_DiffDrive_CollisionGridBased::internal_deinitialize()
{
  m_trajectory.clear();  // Free trajectories
}

void CPTG_DiffDrive_CollisionGridBased::internal_initialize(
    const std::string& cacheFilename, const bool verbose)
{
  using namespace std;

  MRPT_START

  if (verbose)
    cout << endl
         << "[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... "
            "*** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **"
         << endl;

  // Sanity checks:
  ASSERTMSG_(!m_robotShape.empty(), "Robot shape was not defined");
  ASSERTMSG_(m_robotShape.size() >= 3, "Robot shape must have 3 or more vertices");
  ASSERT_(refDistance > 0);
  ASSERT_(V_MAX > 0);
  ASSERT_(W_MAX > 0);
  ASSERT_(m_resolution > 0);

  mrpt::system::CTicTac tictac;
  tictac.Tic();

  if (verbose) cout << "Initializing PTG '" << cacheFilename << "'...";

  // Simulate paths:
  const float min_dist = 0.015f;
  simulateTrajectories(
      100,                          // max.tim,
      refDistance,                  // max.dist,
      10 * refDistance / min_dist,  // max.n,
      0.0005f,                      // diferencial_t
      min_dist                      // min_dist
  );

  // Just for debugging, etc.
  // debugDumpInFiles(n);

  // Check for collisions between the robot shape and the grid cells:
  // ----------------------------------------------------------------------------
  m_collisionGrid.setSize(-refDistance, refDistance, -refDistance, refDistance, m_resolution);

  const size_t Ki = getAlphaValuesCount();
  ASSERTMSG_(Ki > 0, "The PTG seems to be not initialized!");

  // Load the cached version, if possible
  if (loadColGridsFromFile(cacheFilename, m_robotShape))
  {
    if (verbose) cout << "loaded from file OK" << endl;
  }
  else
  {
    // BUGFIX: In case we start reading the file and in the end detected an
    // error,
    //         we must make sure that there's space enough for the grid:
    m_collisionGrid.setSize(
        -refDistance, refDistance, -refDistance, refDistance, m_collisionGrid.getResolution());

    const int grid_cx_max = m_collisionGrid.getSizeX() - 1;
    const int grid_cy_max = m_collisionGrid.getSizeY() - 1;
    const double half_cell = m_collisionGrid.getResolution() * 0.5;

    const size_t nVerts = m_robotShape.verticesCount();
    std::vector<mrpt::math::TPoint2D> transf_shape(nVerts);  // The robot shape at each location

    // RECOMPUTE THE COLLISION GRIDS:
    // ---------------------------------------
    for (size_t k = 0; k < Ki; k++)
    {
      const size_t nPoints = getPathStepCount(k);
      ASSERT_(nPoints > 1);
      for (size_t n = 0; n < (nPoints - 1); n++)
      {
        // Translate and rotate the robot shape at this C-Space pose:
        const mrpt::math::TPose2D p = getPathPose(k, n);

        mrpt::math::TPoint2D bb_min(
            std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
        mrpt::math::TPoint2D bb_max(
            -std::numeric_limits<double>::max(), -std::numeric_limits<double>::max());

        for (size_t m = 0; m < nVerts; m++)
        {
          transf_shape[m].x = p.x + cos(p.phi) * m_robotShape.GetVertex_x(m) -
                              sin(p.phi) * m_robotShape.GetVertex_y(m);
          transf_shape[m].y = p.y + sin(p.phi) * m_robotShape.GetVertex_x(m) +
                              cos(p.phi) * m_robotShape.GetVertex_y(m);
          mrpt::keep_max(bb_max.x, transf_shape[m].x);
          mrpt::keep_max(bb_max.y, transf_shape[m].y);
          mrpt::keep_min(bb_min.x, transf_shape[m].x);
          mrpt::keep_min(bb_min.y, transf_shape[m].y);
        }

        // Robot shape polygon:
        const mrpt::math::TPolygon2D poly(transf_shape);

        // Get the range of cells that may collide with this shape:
        const int ix_min = std::max(0, m_collisionGrid.x2idx(bb_min.x) - 1);
        const int iy_min = std::max(0, m_collisionGrid.y2idx(bb_min.y) - 1);
        const int ix_max = std::min(m_collisionGrid.x2idx(bb_max.x) + 1, grid_cx_max);
        const int iy_max = std::min(m_collisionGrid.y2idx(bb_max.y) + 1, grid_cy_max);

        for (int ix = ix_min; ix < ix_max; ix++)
        {
          const double cx = m_collisionGrid.idx2x(ix) - half_cell;

          for (int iy = iy_min; iy < iy_max; iy++)
          {
            const double cy = m_collisionGrid.idx2y(iy) - half_cell;

            if (poly.contains(mrpt::math::TPoint2D(cx, cy)))
            {
              // Collision!! Update cell info:
              const float d = this->getPathDist(k, n);
              m_collisionGrid.updateCellInfo(ix, iy, k, d);
              m_collisionGrid.updateCellInfo(ix - 1, iy, k, d);
              m_collisionGrid.updateCellInfo(ix, iy - 1, k, d);
              m_collisionGrid.updateCellInfo(ix - 1, iy - 1, k, d);
            }
          }  // for iy
        }    // for ix

      }  // n

      if (verbose)
      {
        cout << k << "/" << Ki << ",";
        cout.flush();
      }
    }  // k

    if (verbose) cout << format("Done! [%.03f sec]", tictac.Tac()) << std::endl;

    // save it to the cache file for the next run:
    saveColGridsToFile(cacheFilename, m_robotShape);

  }  // "else" recompute all PTG

  MRPT_END
}

size_t CPTG_DiffDrive_CollisionGridBased::getPathStepCount(uint16_t k) const
{
  ASSERT_(k < m_trajectory.size());

  return m_trajectory[k].size();
}

mrpt::math::TPose2D CPTG_DiffDrive_CollisionGridBased::getPathPose(uint16_t k, uint32_t step) const
{
  ASSERT_(k < m_trajectory.size());
  ASSERT_(step < m_trajectory[k].size());

  return {m_trajectory[k][step].x, m_trajectory[k][step].y, m_trajectory[k][step].phi};
}

double CPTG_DiffDrive_CollisionGridBased::getPathDist(uint16_t k, uint32_t step) const
{
  ASSERT_(k < m_trajectory.size());
  ASSERT_(step < m_trajectory[k].size());

  return m_trajectory[k][step].dist;
}

bool CPTG_DiffDrive_CollisionGridBased::getPathStepForDist(
    uint16_t k, double dist, uint32_t& out_step) const
{
  ASSERT_(k < m_trajectory.size());
  const size_t numPoints = m_trajectory[k].size();

  ASSERT_(numPoints > 0);

  for (size_t n = 0; n < numPoints - 1; n++)
  {
    if (m_trajectory[k][n + 1].dist >= dist)
    {
      out_step = n;
      return true;
    }
  }

  out_step = numPoints - 1;
  return false;
}

void CPTG_DiffDrive_CollisionGridBased::updateTPObstacle(
    double ox, double oy, std::vector<double>& tp_obstacles) const
{
  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
  // Keep the minimum distance:
  for (const auto& i : cell)
  {
    const double dist = i.second;
    internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacles[i.first]);
  }
}

void CPTG_DiffDrive_CollisionGridBased::updateTPObstacleSingle(
    double ox, double oy, uint16_t k, double& tp_obstacle_k) const
{
  ASSERTMSG_(!m_trajectory.empty(), "PTG has not been initialized!");
  const TCollisionCell& cell = m_collisionGrid.getTPObstacle(ox, oy);
  // Keep the minimum distance:
  for (const auto& i : cell)
    if (i.first == k)
    {
      const double dist = i.second;
      internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
    }
}

void CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(mrpt::serialization::CArchive& in)
{
  CParameterizedTrajectoryGenerator::internal_readFromStream(in);
  CPTG_RobotShape_Polygonal::internal_shape_loadFromStream(in);

  uint8_t version;
  in >> version;
  switch (version)
  {
    case 0:
      internal_deinitialize();
      in >> V_MAX >> W_MAX >> turningRadiusReference >> m_robotShape >> m_resolution >>
          m_trajectory;
      break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

void CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(
    mrpt::serialization::CArchive& out) const
{
  CParameterizedTrajectoryGenerator::internal_writeToStream(out);
  CPTG_RobotShape_Polygonal::internal_shape_saveToStream(out);

  const uint8_t version = 0;
  out << version;

  out << V_MAX << W_MAX << turningRadiusReference << m_robotShape << m_resolution << m_trajectory;
}

mrpt::kinematics::CVehicleVelCmd::Ptr
CPTG_DiffDrive_CollisionGridBased::getSupportedKinematicVelocityCommand() const
{
  return mrpt::kinematics::CVehicleVelCmd::Ptr(new mrpt::kinematics::CVehicleVelCmd_DiffDriven());
}

double CPTG_DiffDrive_CollisionGridBased::getPathStepDuration() const { return m_stepTimeDuration; }

mrpt::math::TTwist2D CPTG_DiffDrive_CollisionGridBased::getPathTwist(
    uint16_t k, uint32_t step) const
{
  ASSERT_(k < m_trajectory.size());
  ASSERT_(step < m_trajectory[k].size());

  auto tw = mrpt::math::TTwist2D(m_trajectory[k][step].v, 0, m_trajectory[k][step].w);
  tw.rotate(m_trajectory[k][step].phi);

  return tw;
}
