/* +------------------------------------------------------------------------+
   |                     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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationRange.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace std;

double COccupancyGridMap2D::internal_computeObservationLikelihood(
    const CObservation& obs, const CPose3D& takenFrom3D) const
{
  // Ignore laser scans if they are not planar or they are not
  //  at the altitude of this grid map:
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    const auto& scan = dynamic_cast<const CObservation2DRangeScan&>(obs);
    if (!scan.isPlanarScan(insertionOptions.horizontalTolerance)) return -10;
    if (insertionOptions.useMapAltitude &&
        fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
      return -10;

    // OK, go on...
  }

  // Execute according to the selected method:
  // --------------------------------------------
  CPose2D takenFrom = CPose2D(takenFrom3D);  // 3D -> 2D, we are in a gridmap...

  switch (likelihoodOptions.likelihoodMethod)
  {
    default:
    case lmRayTracing:
      return computeObservationLikelihood_rayTracing(obs, takenFrom);

    case lmMeanInformation:
      return computeObservationLikelihood_MI(obs, takenFrom);

    case lmConsensus:
      return computeObservationLikelihood_Consensus(obs, takenFrom);

    case lmCellsDifference:
      return computeObservationLikelihood_CellsDifference(obs, takenFrom);

    case lmLikelihoodField_Thrun:
      return computeObservationLikelihood_likelihoodField_Thrun(obs, takenFrom);

    case lmLikelihoodField_II:
      return computeObservationLikelihood_likelihoodField_II(obs, takenFrom);

    case lmConsensusOWA:
      return computeObservationLikelihood_ConsensusOWA(obs, takenFrom);
  };
}

/*---------------------------------------------------------------
      computeObservationLikelihood_Consensus
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  double likResult = 0;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (!IS_CLASS(obs, CObservation2DRangeScan))
  {
    // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
    // classes only.");
    return 1e-3;
  }
  // Observation is a laser range scan:
  // -------------------------------------------
  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);

  // Insert only HORIZONTAL scans, since the grid is supposed to
  //  be a horizontal representation of space.
  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
    return 0.5f;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

  // Assure we have a 2D points-map representation of the points from the
  // scan:
  const auto* compareMap = o.buildAuxPointsMap<mrpt::maps::CPointsMap>();

  // Observation is a points map:
  // -------------------------------------------
  size_t Denom = 0;
  //	int			Acells = 1;
  TPoint2D pointGlobal, pointLocal;

  // Get the points buffers:

  //	compareMap.getPointsBuffer( n, xs, ys, zs );
  const size_t n = compareMap->size();

  for (size_t i = 0; i < n; i += likelihoodOptions.consensus_takeEachRange)
  {
    // Get the point and pass it to global coordinates:
    compareMap->getPoint(i, pointLocal);
    takenFrom.composePoint(pointLocal, pointGlobal);

    int cx0 = x2idx(pointGlobal.x);
    int cy0 = y2idx(pointGlobal.y);

    likResult += 1 - getCell_nocheck(cx0, cy0);
    Denom++;
  }
  if (Denom) likResult /= Denom;
  likResult = pow(likResult, static_cast<double>(likelihoodOptions.consensus_pow));

  return log(likResult);
}

/*---------------------------------------------------------------
      computeObservationLikelihood_ConsensusOWA
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  double likResult = 0;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
    // classes only.");
    return 1e-3;
  }
  // Observation is a laser range scan:
  // -------------------------------------------
  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);

  // Insert only HORIZONTAL scans, since the grid is supposed to
  //  be a horizontal representation of space.
  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
    return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

  // Assure we have a 2D points-map representation of the points from the
  // scan:
  CPointsMap::TInsertionOptions insOpt;
  insOpt.minDistBetweenLaserPoints = -1;  // ALL the laser points

  const auto* compareMap = o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&insOpt);

  // Observation is a points map:
  // -------------------------------------------
  int Acells = 1;
  TPoint2D pointGlobal, pointLocal;

  // Get the points buffers:
  const size_t n = compareMap->size();

  // Store the likelihood values in this vector:
  likelihoodOutputs.OWA_pairList.clear();
  for (size_t i = 0; i < n; i++)
  {
    // Get the point and pass it to global coordinates:
    compareMap->getPoint(i, pointLocal);
    takenFrom.composePoint(pointLocal, pointGlobal);

    int cx0 = x2idx(pointGlobal.x);
    int cy0 = y2idx(pointGlobal.y);

    int cxMin = max(0, cx0 - Acells);
    int cxMax = min(static_cast<int>(m_size_x) - 1, cx0 + Acells);
    int cyMin = max(0, cy0 - Acells);
    int cyMax = min(static_cast<int>(m_size_y) - 1, cy0 + Acells);

    double lik = 0;

    for (int cx = cxMin; cx <= cxMax; cx++)
      for (int cy = cyMin; cy <= cyMax; cy++) lik += 1 - getCell_nocheck(cx, cy);

    int nCells = (cxMax - cxMin + 1) * (cyMax - cyMin + 1);
    ASSERT_(nCells > 0);
    lik /= nCells;

    TPairLikelihoodIndex element;
    element.first = lik;
    element.second = pointGlobal;
    likelihoodOutputs.OWA_pairList.push_back(element);
  }  // for each range point

  // Sort the list of likelihood values, in descending order:
  // ------------------------------------------------------------
  std::sort(likelihoodOutputs.OWA_pairList.begin(), likelihoodOutputs.OWA_pairList.end());

  // Cut the vector to the highest "likelihoodOutputs.OWA_length" elements:
  size_t M = likelihoodOptions.OWA_weights.size();
  ASSERT_(likelihoodOutputs.OWA_pairList.size() >= M);

  likelihoodOutputs.OWA_pairList.resize(M);
  likelihoodOutputs.OWA_individualLikValues.resize(M);
  likResult = 0;
  for (size_t k = 0; k < M; k++)
  {
    likelihoodOutputs.OWA_individualLikValues[k] = likelihoodOutputs.OWA_pairList[k].first;
    likResult += likelihoodOptions.OWA_weights[k] * likelihoodOutputs.OWA_individualLikValues[k];
  }

  return log(likResult);
}

/*---------------------------------------------------------------
      computeObservationLikelihood_CellsDifference
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_CellsDifference(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  double ret = 0.5;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
      return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

    // Build a copy of this occupancy grid:
    COccupancyGridMap2D compareGrid(
        takenFrom.x() - 10, takenFrom.x() + 10, takenFrom.y() - 10, takenFrom.y() + 10,
        m_resolution);
    CPose3D robotPose(takenFrom);
    int Ax, Ay;

    // Insert in this temporary grid:
    compareGrid.insertionOptions.maxDistanceInsertion = insertionOptions.maxDistanceInsertion;
    compareGrid.insertionOptions.maxOccupancyUpdateCertainty = 0.95f;
    o.insertObservationInto(compareGrid, robotPose);

    // Save Cells offset between the two grids:
    Ax = round((m_xMin - compareGrid.m_xMin) / m_resolution);
    Ay = round((m_yMin - compareGrid.m_yMin) / m_resolution);

    int nCellsCompared = 0;
    float cellsDifference = 0;
    int x0 = max(0, Ax);
    int y0 = max(0, Ay);
    int x1 = min(compareGrid.m_size_x, m_size_x + Ax);
    int y1 = min(compareGrid.m_size_y, m_size_y + Ay);

    for (int x = x0; x < x1; x += 1)
    {
      for (int y = y0; y < y1; y += 1)
      {
        float xx = compareGrid.idx2x(x);
        float yy = compareGrid.idx2y(y);

        float c1 = getPos(xx, yy);
        float c2 = compareGrid.getCell(x, y);
        if (c2 < 0.45f || c2 > 0.55f)
        {
          nCellsCompared++;
          if ((c1 > 0.5 && c2 < 0.5) || (c1 < 0.5 && c2 > 0.5)) cellsDifference++;
        }
      }
    }
    ret = 1 - cellsDifference / (nCellsCompared);
  }
  return log(ret);
}

/*---------------------------------------------------------------
      computeObservationLikelihood_MI
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_MI(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  MRPT_START

  CPose3D poseRobot(takenFrom);
  double res;

  // Dont modify the grid, only count the changes in Information
  updateInfoChangeOnly.enabled = true;
  const_cast<COccupancyGridMap2D*>(this)->insertionOptions.maxDistanceInsertion *=
      likelihoodOptions.MI_ratio_max_distance;

  // Reset the new information counters:
  updateInfoChangeOnly.cellsUpdated = 0;
  updateInfoChangeOnly.I_change = 0;
  updateInfoChangeOnly.laserRaysSkip = likelihoodOptions.MI_skip_rays;

  // Insert the observation (It will not be really inserted, only the
  // information counted)
  const_cast<COccupancyGridMap2D*>(this)->insertObservation(obs, poseRobot);

  // Compute the change in I aported by the observation:
  double newObservation_mean_I;
  if (updateInfoChangeOnly.cellsUpdated)
    newObservation_mean_I = updateInfoChangeOnly.I_change / updateInfoChangeOnly.cellsUpdated;
  else
    newObservation_mean_I = 0;

  // Let the normal mode enabled, i.e. the grid can be updated
  updateInfoChangeOnly.enabled = false;
  const_cast<COccupancyGridMap2D*>(this)->insertionOptions.maxDistanceInsertion /=
      likelihoodOptions.MI_ratio_max_distance;

  res = pow(newObservation_mean_I, static_cast<double>(likelihoodOptions.MI_exponent));

  return log(res);

  MRPT_END
}

double COccupancyGridMap2D::computeObservationLikelihood_rayTracing(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  double ret = 0;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
    CObservation2DRangeScan simulatedObs;

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
      return 0.5;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

    // The number of simulated rays will be original range scan rays /
    // DOWNRATIO
    int decimation = likelihoodOptions.rayTracing_decimation;
    int nRays = o.getScanSize();

    // Perform simulation using same parameters than real observation:
    simulatedObs.aperture = o.aperture;
    simulatedObs.maxRange = o.maxRange;
    simulatedObs.rightToLeft = o.rightToLeft;
    simulatedObs.sensorPose = o.sensorPose;

    // Performs the scan simulation:
    laserScanSimulator(
        simulatedObs,  // The in/out observation
        takenFrom,     // robot pose
        0.45f,         // Cells threshold
        nRays,         // Scan length
        0, decimation);

    double stdLaser = likelihoodOptions.rayTracing_stdHit;
    double stdSqrt2 = sqrt(2.0f) * stdLaser;

    // Compute likelihoods:
    ret = 1;
    // bool		useDF = likelihoodOptions.rayTracing_useDistanceFilter;
    float r_sim, r_obs;
    double likelihood;

    for (int j = 0; j < nRays; j += decimation)
    {
      // Simulated and measured ranges:
      r_sim = simulatedObs.getScanRange(j);
      r_obs = o.getScanRange(j);

      // Is a valid range?
      if (o.getScanRangeValidity(j))
      {
        likelihood =
            0.1 / o.maxRange + 0.9 * exp(-square(min((float)fabs(r_sim - r_obs), 2.0f) / stdSqrt2));
        ret += log(likelihood);
        // printf("Sim=%f\tReal=%f\tlik=%f\n",r_sim,r_obs,likelihood);
      }
    }
  }

  return ret;
}
/**/

/*---------------------------------------------------------------
      computeObservationLikelihood_likelihoodField_Thrun
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_Thrun(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  MRPT_START

  double ret = 0;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance)) return -10;

    // Assure we have a 2D points-map representation of the points from the
    // scan:
    CPointsMap::TInsertionOptions opts;
    opts.minDistBetweenLaserPoints = m_resolution * 0.5f;
    opts.isPlanarMap = true;  // Already filtered above!
    opts.horizontalTolerance = insertionOptions.horizontalTolerance;

    // Compute the likelihood of the points in this grid map:
    ret = computeLikelihoodField_Thrun(
        o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&opts), &takenFrom);

  }  // end of observation is a scan range 2D
  else if (IS_CLASS(obs, CObservationRange))
  {
    // Sonar-like observations:
    // ---------------------------------------
    const auto& o = dynamic_cast<const CObservationRange&>(obs);

    // Create a point map representation of the observation:
    CSimplePointsMap pts;
    pts.insertionOptions.minDistBetweenLaserPoints = m_resolution * 0.5f;
    pts.insertObservation(o);

    // Compute the likelihood of the points in this grid map:
    ret = computeLikelihoodField_Thrun(&pts, &takenFrom);
  }

  return ret;

  MRPT_END
}

/*---------------------------------------------------------------
    computeObservationLikelihood_likelihoodField_II
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_II(
    const CObservation& obs, const CPose2D& takenFrom) const
{
  MRPT_START

  double ret = 0;

  // This function depends on the observation type:
  // -----------------------------------------------------
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    // Observation is a laser range scan:
    // -------------------------------------------
    const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);

    // Insert only HORIZONTAL scans, since the grid is supposed to
    //  be a horizontal representation of space.
    if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
      return 0.5f;  // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!

    // Assure we have a 2D points-map representation of the points from the
    // scan:

    // Compute the likelihood of the points in this grid map:
    ret = computeLikelihoodField_II(o.buildAuxPointsMap<mrpt::maps::CPointsMap>(), &takenFrom);

  }  // end of observation is a scan range 2D

  return ret;

  MRPT_END
}

/*---------------------------------------------------------------
          computeLikelihoodField_Thrun
 ---------------------------------------------------------------*/
double COccupancyGridMap2D::computeLikelihoodField_Thrun(
    const CPointsMap* pm, const CPose2D* relativePose) const
{
  MRPT_START

  double ret;
  size_t N = pm->size();
  int K = (int)ceil(
      likelihoodOptions.LF_maxCorrsDistance /*m*/ /
      m_resolution);  // The size of the checking area for matchings:

  bool Product_T_OrSum_F = !likelihoodOptions.LF_alternateAverageMethod;

  if (!N)
  {
    return -100;  // No way to estimate this likelihood!!
  }

  // Compute the likelihoods for each point:
  ret = 0;

  float stdHit = likelihoodOptions.LF_stdHit;
  float zHit = likelihoodOptions.LF_zHit;
  float zRandom = likelihoodOptions.LF_zRandom;
  float zRandomMaxRange = likelihoodOptions.LF_maxRange;
  float zRandomTerm = zRandom / zRandomMaxRange;
  float Q = -0.5f / square(stdHit);
  int M = 0;

  unsigned int size_x_1 = m_size_x - 1;
  unsigned int size_y_1 = m_size_y - 1;

#define LIK_LF_CACHE_INVALID (66)

  // Aux. variables for the "for j" loop:
  double thisLik = LIK_LF_CACHE_INVALID;
  double maxCorrDist_sq = square(likelihoodOptions.LF_maxCorrsDistance);
  double minimumLik = zRandomTerm + zHit * exp(Q * maxCorrDist_sq);
  double ccos, ssin;

  if (likelihoodOptions.enableLikelihoodCache)
  {
    // Reset the precomputed likelihood values map
    if (m_likelihoodCacheOutDated)
    {
      if (!m_map.empty())
        m_precomputedLikelihood.assign(m_map.size(), LIK_LF_CACHE_INVALID);
      else
        m_precomputedLikelihood.clear();

      m_likelihoodCacheOutDated = false;
    }
  }

  cellType thresholdCellValue = p2l(0.5f);
  int decimation = likelihoodOptions.LF_decimation;

  const double _resolution = this->m_resolution;
  const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
  const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;

  if (N < 10) decimation = 1;

  TPoint2D pointLocal;
  TPoint2D pointGlobal;

  for (size_t j = 0; j < N; j += decimation)
  {
    // Get the point and pass it to global coordinates:
    if (relativePose)
    {
      pm->getPoint(j, pointLocal);
// pointGlobal = *relativePose + pointLocal;
#ifdef HAVE_SINCOS
      ::sincos(relativePose->phi(), &ssin, &ccos);
#else
      ccos = cos(relativePose->phi());
      ssin = sin(relativePose->phi());
#endif
      pointGlobal.x = relativePose->x() + pointLocal.x * ccos - pointLocal.y * ssin;
      pointGlobal.y = relativePose->y() + pointLocal.x * ssin + pointLocal.y * ccos;
    }
    else
    {
      pm->getPoint(j, pointGlobal);
    }

    // Point to cell indixes
    int cx = x2idx(pointGlobal.x);
    int cy = y2idx(pointGlobal.y);

    // Precomputed table:
    // Tip: Comparison cx<0 is implicit in (unsigned)(x)>size...
    if (static_cast<unsigned>(cx) >= size_x_1 || static_cast<unsigned>(cy) >= size_y_1)
    {
      // We are outside of the map: Assign the likelihood for the max.
      // correspondence distance:
      thisLik = minimumLik;
    }
    else
    {
      // We are into the map limits:
      if (likelihoodOptions.enableLikelihoodCache)
      {
        thisLik = m_precomputedLikelihood[cx + cy * m_size_x];
      }

      if (!likelihoodOptions.enableLikelihoodCache || thisLik == LIK_LF_CACHE_INVALID)
      {
        // Compute now:
        // -------------
        // Find the closest occupied cell in a certain range, given by
        // K:
        int xx1 = max(0, cx - K);
        int xx2 = min(size_x_1, (unsigned)(cx + K));
        int yy1 = max(0, cy - K);
        int yy2 = min(size_y_1, (unsigned)(cy + K));

        // Optimized code: this part will be invoked a *lot* of times:
        float occupiedMinDist;
        {
          // Initial pointer position
          const cellType* mapPtr = &m_map[xx1 + yy1 * m_size_x];
          unsigned incrAfterRow = m_size_x - ((xx2 - xx1) + 1);

          signed int Ax0 = 10 * (xx1 - cx);
          signed int Ay = 10 * (yy1 - cy);

          unsigned int occupiedMinDistInt = mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);

          for (int yy = yy1; yy <= yy2; yy++)
          {
            unsigned int Ay2 = square((unsigned int)(Ay));  // Square is faster
            // with unsigned.
            signed short Ax = Ax0;
            cellType cell;

            for (int xx = xx1; xx <= xx2; xx++)
            {
              if ((cell = *mapPtr++) < thresholdCellValue)
              {
                unsigned int d = square((unsigned int)(Ax)) + Ay2;
                keep_min(occupiedMinDistInt, d);
              }
              Ax += 10;
            }
            // Go to (xx1,yy++)
            mapPtr += incrAfterRow;
            Ay += 10;
          }

          occupiedMinDist = occupiedMinDistInt * constDist2DiscrUnits_INV;
        }

        if (likelihoodOptions.LF_useSquareDist) occupiedMinDist *= occupiedMinDist;

        thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);

        if (likelihoodOptions.enableLikelihoodCache)
          // And save it into the table and into "thisLik":
          m_precomputedLikelihood[cx + cy * m_size_x] = thisLik;
      }
    }

    // Update the likelihood:
    if (Product_T_OrSum_F)
    {
      ret += log(thisLik);
    }
    else
    {
      ret += thisLik;
      M++;
    }
  }  // end of for each point in the scan

  if (!Product_T_OrSum_F) ret = log(ret / M);

  return ret;

  MRPT_END
}

/*---------------------------------------------------------------
          computeLikelihoodField_II
 ---------------------------------------------------------------*/
double COccupancyGridMap2D::computeLikelihoodField_II(
    const CPointsMap* pm, const CPose2D* relativePose) const
{
  MRPT_START

  double ret;
  size_t N = pm->size();

  if (!N) return 1e-100;  // No way to estimate this likelihood!!

  // Compute the likelihoods for each point:
  ret = 0;
  //	if (likelihoodOptions.LF_alternateAverageMethod)
  //			ret = 0;
  //	else	ret = 1;

  TPoint2D pointLocal, pointGlobal;

  float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
  float Q = -0.5f / square(likelihoodOptions.LF_stdHit);

  // Aux. cell indixes variables:
  int cx, cy;
  size_t j;
  int cx0, cy0;
  int cx_min, cx_max;
  int cy_min, cy_max;
  int maxRangeInCells = (int)ceil(likelihoodOptions.LF_maxCorrsDistance / m_resolution);
  int nCells = 0;

  // -----------------------------------------------------
  // Compute around a window of neigbors around each point
  // -----------------------------------------------------
  for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
  {
    // Get the point and pass it to global coordinates:
    // ---------------------------------------------
    if (relativePose)
    {
      pm->getPoint(j, pointLocal);
      pointGlobal = *relativePose + pointLocal;
    }
    else
    {
      pm->getPoint(j, pointGlobal);
    }

    // Point to cell indixes:
    // ---------------------------------------------
    cx0 = x2idx(pointGlobal.x);
    cy0 = y2idx(pointGlobal.y);

    // Compute the range of cells to compute:
    // ---------------------------------------------
    cx_min = max(cx0 - maxRangeInCells, 0);
    cx_max = min(cx0 + maxRangeInCells, static_cast<int>(m_size_x));
    cy_min = max(cy0 - maxRangeInCells, 0);
    cy_max = min(cy0 + maxRangeInCells, static_cast<int>(m_size_y));

    //		debugImg.rectangle(cx_min,cy_min,cx_max,cy_max,0xFF0000 );

    // Compute over the window of cells:
    // ---------------------------------------------
    double lik = 0;
    for (cx = cx_min; cx <= cx_max; cx++)
    {
      for (cy = cy_min; cy <= cy_max; cy++)
      {
        float P_free = getCell(cx, cy);
        float termDist =
            exp(Q * (square(idx2x(cx) - pointGlobal.x) + square(idx2y(cy) - pointGlobal.y)));

        lik += P_free * zRandomTerm + (1 - P_free) * termDist;
      }  // end for cy
    }    // end for cx

    // Update the likelihood:
    if (likelihoodOptions.LF_alternateAverageMethod)
      ret += lik;
    else
      ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
    nCells++;

  }  // end of for each point in the scan

  if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
    ret = log(ret / nCells);
  else
    ret = ret / nCells;

  return ret;

  MRPT_END
}

/*---------------------------------------------------------------
  Initialization of values, don't needed to be called directly.
  ---------------------------------------------------------------*/
COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions() : OWA_weights(100, 1 / 100.0f) {}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TLikelihoodOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
  likelihoodMethod = iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);

  enableLikelihoodCache =
      iniFile.read_bool(section, "enableLikelihoodCache", enableLikelihoodCache);

  LF_stdHit = iniFile.read_float(section, "LF_stdHit", LF_stdHit);
  LF_zHit = iniFile.read_float(section, "LF_zHit", LF_zHit);
  LF_zRandom = iniFile.read_float(section, "LF_zRandom", LF_zRandom);
  LF_maxRange = iniFile.read_float(section, "LF_maxRange", LF_maxRange);
  LF_decimation = iniFile.read_int(section, "LF_decimation", LF_decimation);
  LF_maxCorrsDistance = iniFile.read_float(section, "LF_maxCorrsDistance", LF_maxCorrsDistance);
  LF_useSquareDist = iniFile.read_bool(section, "LF_useSquareDist", LF_useSquareDist);
  LF_alternateAverageMethod =
      iniFile.read_bool(section, "LF_alternateAverageMethod", LF_alternateAverageMethod);

  MI_exponent = iniFile.read_float(section, "MI_exponent", MI_exponent);
  MI_skip_rays = iniFile.read_int(section, "MI_skip_rays", MI_skip_rays);
  MI_ratio_max_distance =
      iniFile.read_float(section, "MI_ratio_max_distance", MI_ratio_max_distance);

  rayTracing_useDistanceFilter =
      iniFile.read_bool(section, "rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
  rayTracing_stdHit = iniFile.read_float(section, "rayTracing_stdHit", rayTracing_stdHit);

  consensus_takeEachRange =
      iniFile.read_int(section, "consensus_takeEachRange", consensus_takeEachRange);
  consensus_pow = iniFile.read_float(section, "consensus_pow", consensus_pow);

  iniFile.read_vector(section, "OWA_weights", OWA_weights, OWA_weights);
}

/*---------------------------------------------------------------
          dumpToTextStream
  ---------------------------------------------------------------*/
void COccupancyGridMap2D::TLikelihoodOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] "
         "------------ "
         "\n\n";

  out << "likelihoodMethod                        = ";
  switch (likelihoodMethod)
  {
    case lmMeanInformation:
      out << "lmMeanInformation";
      break;
    case lmRayTracing:
      out << "lmRayTracing";
      break;
    case lmConsensus:
      out << "lmConsensus";
      break;
    case lmCellsDifference:
      out << "lmCellsDifference";
      break;
    case lmLikelihoodField_Thrun:
      out << "lmLikelihoodField_Thrun";
      break;
    case lmLikelihoodField_II:
      out << "lmLikelihoodField_II";
      break;
    case lmConsensusOWA:
      out << "lmConsensusOWA";
      break;
    default:
      out << "UNKNOWN!!!";
      break;
  }
  out << "\n";

  out << mrpt::format(
      "enableLikelihoodCache                   = %c\n", enableLikelihoodCache ? 'Y' : 'N');

  out << mrpt::format("LF_stdHit                               = %f\n", LF_stdHit);
  out << mrpt::format("LF_zHit                                 = %f\n", LF_zHit);
  out << mrpt::format("LF_zRandom                              = %f\n", LF_zRandom);
  out << mrpt::format("LF_maxRange                             = %f\n", LF_maxRange);
  out << mrpt::format("LF_decimation                           = %u\n", LF_decimation);
  out << mrpt::format("LF_maxCorrsDistance                     = %f\n", LF_maxCorrsDistance);
  out << mrpt::format(
      "LF_useSquareDist                        = %c\n", LF_useSquareDist ? 'Y' : 'N');
  out << mrpt::format(
      "LF_alternateAverageMethod               = %c\n", LF_alternateAverageMethod ? 'Y' : 'N');
  out << mrpt::format("MI_exponent                             = %f\n", MI_exponent);
  out << mrpt::format("MI_skip_rays                            = %u\n", MI_skip_rays);
  out << mrpt::format("MI_ratio_max_distance                   = %f\n", MI_ratio_max_distance);
  out << mrpt::format(
      "rayTracing_useDistanceFilter            = %c\n", rayTracing_useDistanceFilter ? 'Y' : 'N');
  out << mrpt::format("rayTracing_decimation                   = %u\n", rayTracing_decimation);
  out << mrpt::format("rayTracing_stdHit                       = %f\n", rayTracing_stdHit);
  out << mrpt::format("consensus_takeEachRange                 = %u\n", consensus_takeEachRange);
  out << mrpt::format("consensus_pow                           = %.02f\n", consensus_pow);
  out << "OWA_weights   = [";
  for (size_t i = 0; i < OWA_weights.size(); i++)
  {
    if (i < 3 || i > (OWA_weights.size() - 3))
      out << mrpt::format("%.03f ", OWA_weights[i]);
    else if (i == 3 && OWA_weights.size() > 6)
      out << " ... ";
  }
  out << mrpt::format("] (size=%u)\n", (unsigned)OWA_weights.size());
  out << "\n";
}

/** Returns true if this map is able to compute a sensible likelihood function
 * for this observation (i.e. an occupancy grid map cannot with an image).
 * \param obs The observation.
 * \sa computeObservationLikelihood
 */
bool COccupancyGridMap2D::internal_canComputeObservationLikelihood(
    const mrpt::obs::CObservation& obs) const
{
  // Ignore laser scans if they are not planar or they are not
  //  at the altitude of this grid map:
  if (IS_CLASS(obs, CObservation2DRangeScan))
  {
    const auto& scan = dynamic_cast<const CObservation2DRangeScan&>(obs);

    if (!scan.isPlanarScan(insertionOptions.horizontalTolerance)) return false;
    if (insertionOptions.useMapAltitude &&
        fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
      return false;

    // OK, go on...
    return true;
  }
  else  // Is not a laser scanner...
  {
    return false;
  }
}
