/* +------------------------------------------------------------------------+
   |                     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/config/CLoadableOptions.h>
#include <mrpt/maps/CLogOddsGridMap3D.h>
#include <mrpt/maps/CLogOddsGridMapLUT.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/maps/NearestNeighborsCapable.h>
#include <mrpt/maps/OccupancyGridCellType.h>
#include <mrpt/opengl/opengl_frwds.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/typemeta/TEnumType.h>

namespace mrpt::obs
{
class CObservationPointCloud;
}

namespace mrpt::maps
{
/** A 3D occupancy grid map with a regular, even distribution of voxels.
 *
 * This is a faster alternative to COctoMap, but use with caution with limited
 *map extensions, since it could easily exaust available memory.
 *
 * Each voxel follows a Bernoulli probability distribution: a value of 0 means
 *certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means
 *uncertainty.
 *
 * An alternative, sparse representation of a 3D map is provided
 * via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB
 *
 * \ingroup mrpt_maps_grp
 **/
class COccupancyGridMap3D :
    public CMetricMap,
    public CLogOddsGridMap3D<OccGridCellTraits::cellType>,
    public mrpt::maps::NearestNeighborsCapable
{
  DEFINE_SERIALIZABLE(COccupancyGridMap3D, mrpt::maps)
 public:
  /** The type of the map voxels: */
  using voxelType = OccGridCellTraits::cellType;
  using voxelTypeUnsigned = OccGridCellTraits::cellTypeUnsigned;

 protected:
  /** Lookup tables for log-odds */
  static CLogOddsGridMapLUT<voxelType>& get_logodd_lut();

  /** True upon construction; used by isEmpty() */
  bool m_is_empty{true};

  /** See base class */
  void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation&) override;

  /** Clear the map: It set all voxels to their default occupancy value (0.5),
   * without changing the resolution (the grid extension is reset to the
   * default values). */
  void internal_clear() override;

  /** Insert the observation information into this map.
   *
   * \param obs The observation
   * \param robotPose The 3D pose of the robot mobile base in the map
   * reference system, or nullptr (default) if you want to use
   * CPose2D(0,0,deg)
   *
   * \sa insertionOptions, CObservation::insertObservationInto
   */
  bool internal_insertObservation(
      const mrpt::obs::CObservation& obs,
      const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt) override;

  void internal_insertObservationScan2D(
      const mrpt::obs::CObservation2DRangeScan& o, const mrpt::poses::CPose3D& robotPose);

  void internal_insertObservationScan3D(
      const mrpt::obs::CObservation3DRangeScan& o, const mrpt::poses::CPose3D& robotPose);

  void internal_insertObservationPointCloud(
      const mrpt::obs::CObservationPointCloud& o, const mrpt::poses::CPose3D& robotPose);

 public:
  /** Constructor */
  COccupancyGridMap3D(
      const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
      const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
      float resolution = 0.25f);

  /** Fills all the voxels with a default value. */
  void fill(float default_value = 0.5f);

  /** Change the size of gridmap, erasing all its previous contents.
   * \param resolution The new size of voxels.
   * \param default_value The value of voxels, typically 0.5.
   * \sa ResizeGrid
   */
  void setSize(
      const mrpt::math::TPoint3D& corner_min,
      const mrpt::math::TPoint3D& corner_max,
      double resolution,
      float default_value = 0.5f);

  /** Change the size of gridmap, maintaining previous contents.
   * \param new_voxels_default_value Value of new voxels, typically 0.5
   * \sa setSize()
   */
  void resizeGrid(
      const mrpt::math::TPoint3D& corner_min,
      const mrpt::math::TPoint3D& corner_max,
      float new_voxels_default_value = 0.5f);

  /** Scales an integer representation of the log-odd into a real valued
   * probability in [0,1], using p=exp(l)/(1+exp(l))  */
  static inline float l2p(const voxelType l) { return get_logodd_lut().l2p(l); }
  /** Scales an integer representation of the log-odd into a linear scale
   * [0,255], using p=exp(l)/(1+exp(l)) */
  static inline uint8_t l2p_255(const voxelType l) { return get_logodd_lut().l2p_255(l); }
  /** Scales a real valued probability in [0,1] to an integer representation
   * of: log(p)-log(1-p)  in the valid range of voxelType */
  static inline voxelType p2l(const float p) { return get_logodd_lut().p2l(p); }

  /** Performs the Bayesian fusion of a new observation of a cell  \sa
   * updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
  void updateCell(int cx_idx, int cy_idx, int cz_idx, float v);

  /** Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its
   * index. */
  inline void setCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz, float value)
  {
    if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr) *c = p2l(value);
  }

  /** Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel,
   * given its index */
  inline float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const
  {
    if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr)
      return l2p(*c);
    else
      return .5f;
  }

  /** Change the contents [0,1] of a voxel, given its coordinates */
  inline void setFreenessByPos(float x, float y, float z, float value)
  {
    setCellFreeness(m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z), value);
  }

  /** Read the real valued [0,1] contents of a voxel, given its coordinates */
  inline float getFreenessByPos(float x, float y, float z) const
  {
    return getCellFreeness(m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z));
  }

  /** Increases the freeness of a ray segment, and the occupancy of the voxel
   * at its end point (unless endIsOccupied=false).
   * Normally, users would prefer the higher-level method
   * CMetricMap::insertObservation()
   */
  void insertRay(
      const mrpt::math::TPoint3D& sensor,
      const mrpt::math::TPoint3D& end,
      bool endIsOccupied = true);

  /** Calls insertRay() for each point in the point cloud, using as sensor
   * central point (the origin of all rays), the given `sensorCenter`.
   * \param[in] maxValidRange If a point has larger distance from
   * `sensorCenter` than `maxValidRange`, it will be considered a non-echo,
   * and NO occupied voxel will be created at the end of the segment.
   * \sa insertionOptions parameters are observed in this method.
   */
  void insertPointCloud(
      const mrpt::math::TPoint3D& sensorCenter,
      const mrpt::maps::CPointsMap& pts,
      const float maxValidRange = std::numeric_limits<float>::max(),
      const std::optional<mrpt::poses::CPose3D>& robotPose = std::nullopt);

  /** \sa renderingOptions */
  void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const;

  /** Returns a 3D object representing the map. \sa renderingOptions */
  void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const override;

  mrpt::math::TBoundingBoxf boundingBox() const override
  {
    return {
        mrpt::math::TPoint3Df(m_grid.getXMin(), m_grid.getYMin(), m_grid.getZMin()),
        mrpt::math::TPoint3Df(m_grid.getXMax(), m_grid.getYMax(), m_grid.getZMax())};
  }

  /** With this struct options are provided to the observation insertion
   * process.
   * \sa CObservation::insertIntoGridMap */
  class TInsertionOptions : public mrpt::config::CLoadableOptions
  {
   public:
    TInsertionOptions() = default;

    /** This method load the options from a ".ini" file.
     *   Only those parameters found in the given "section" and having
     *   the same name that the variable are loaded. Those not found in
     *   the file will stay with their previous values (usually the default
     *   values loaded at initialization). An example of an ".ini" file:
     *  \code
     *  [section]
     *	resolution=0.10		; blah blah...
     *	modeSelection=1		; 0=blah, 1=blah,...
     *  \endcode
     */
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source, const std::string& section) override;
    // See base docs
    void saveToConfigFile(
        mrpt::config::CConfigFileBase& target, const std::string& section) const override;

    /** Largest distance at which voxels are updated (Default: 15 meters) */
    float maxDistanceInsertion{15.0f};
    /** A value in the range [0.5,1] used for updating voxel with a Bayesian
     * approach (default 0.65) */
    float maxOccupancyUpdateCertainty{0.65f};
    /** A value in the range [0.5,1] for updating a free voxel. (default=0
     * means use the same than maxOccupancyUpdateCertainty) */
    float maxFreenessUpdateCertainty{.0f};
    /** Specify the decimation of 3D range scans.
     * "N" means keeping the minimum range of each block of "NxN" range
     * pixels.
     * (default=8)
     */
    uint16_t decimation_3d_range{8};

    /** Decimation for insertPointCloud() or 2D range scans (Default: 1) */
    uint16_t decimation{1};

    /** If true, raytrace and fill empty cells. If false, only the final
     * (end point) of each ray will be marked as occupied.
     */
    bool raytraceEmptyCells = true;
  };

  /** With this struct options are provided to the observation insertion
   * process \sa CObservation::insertIntoGridMap */
  TInsertionOptions insertionOptions;

  /** The type for selecting a likelihood computation method */
  enum TLikelihoodMethod
  {
    lmLikelihoodField_Thrun = 0,
    lmRayTracing
    // Remember: Update TEnumType below if new values are added here!
  };

  /** With this struct options are provided to the observation likelihood
   * computation process */
  class TLikelihoodOptions : public mrpt::config::CLoadableOptions
  {
   public:
    TLikelihoodOptions() = default;

    /** This method load the options from a ".ini" file.
     *   Only those parameters found in the given "section" and having
     *   the same name that the variable are loaded. Those not found in
     *   the file will stay with their previous values (usually the default
     *   values loaded at initialization). An example of an ".ini" file:
     *  \code
     *  [section]
     *	resolution=0.10		; blah blah...
     *	modeSelection=1		; 0=blah, 1=blah,...
     *  \endcode
     */
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source, const std::string& section) override;
    // See base docs
    void saveToConfigFile(mrpt::config::CConfigFileBase& c, const std::string& s) const override;

    /** The selected method to compute an observation likelihood */
    TLikelihoodMethod likelihoodMethod{lmLikelihoodField_Thrun};
    /** [LikelihoodField] The laser range "sigma" used in computations;
     * Default value = 0.35 */
    float LF_stdHit{0.35f};
    /** [LikelihoodField] */
    float LF_zHit{0.95f}, LF_zRandom{0.05f};
    /** [LikelihoodField] The max. range of the sensor (Default= 81 m) */
    float LF_maxRange{20.0f};
    /** [LikelihoodField] The decimation of the points in a scan, default=1
     * == no decimation */
    uint32_t LF_decimation{1};
    /** [LikelihoodField] The max. distance for searching correspondences
     * around each sensed point */
    float LF_maxCorrsDistance{0.3f};
    /** [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of
     * `exp(dist^2/std^2)` */
    bool LF_useSquareDist{false};
    /** [rayTracing] One out of "rayTracing_decimation" rays will be
     * simulated and compared only: set to 1 to use all the sensed ranges.
     */
    int32_t rayTracing_decimation{10};
    /** [rayTracing] The laser range sigma. */
    float rayTracing_stdHit{1.0f};
  };

  TLikelihoodOptions likelihoodOptions;

  /** Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a
   * mrpt::opengl::COctoMapVoxels */
  struct TRenderingOptions
  {
    TRenderingOptions() = default;

    /** Generate grid lines for all octree nodes, useful to draw the
     "structure" of the
     octree, but computationally costly (Default: false) */
    bool generateGridLines{false};

    /** Generate voxels for the occupied volumes  (Default=true) */
    bool generateOccupiedVoxels{true};

    /** Set occupied voxels visible (requires generateOccupiedVoxels=true)
     * (Default=true)*/
    bool visibleOccupiedVoxels{true};

    /** Generate voxels for the freespace (Default=true) */
    bool generateFreeVoxels{true};

    /** Set free voxels visible (requires generateFreeVoxels=true)
     * (Default=true) */
    bool visibleFreeVoxels{true};

    /** Binary dump to stream */
    void writeToStream(mrpt::serialization::CArchive& out) const;
    /** Binary dump to stream */
    void readFromStream(mrpt::serialization::CArchive& in);
  };

  TRenderingOptions renderingOptions;

  /** Returns true upon map construction or after calling clear(), the return
   *  changes to false upon successful insertObservation() or any other
   * method to load data in the map.
   */
  bool isEmpty() const override;

  /** See docs in base class: in this class this always returns 0 */
  void determineMatching2D(
      const mrpt::maps::CMetricMap* otherMap,
      const mrpt::poses::CPose2D& otherMapPose,
      mrpt::tfest::TMatchingPairList& correspondences,
      const TMatchingParams& params,
      TMatchingExtraResults& extraResults) const override;

  /** See docs in base class: in this class this always returns 0 */
  float compute3DMatchingRatio(
      const mrpt::maps::CMetricMap* otherMap,
      const mrpt::poses::CPose3D& otherMapPose,
      const TMatchingRatioParams& params) const override;

  void saveMetricMapRepresentationToFile(const std::string& f) const override;

  /** Returns a short description of the map. */
  std::string asString() const override
  {
    return mrpt::format(
        "3D gridmap, extending from (%f,%f,%f) to (%f,%f,%f), voxel "
        "resolution in XY=%f in Z=%f",
        m_grid.getXMin(), m_grid.getYMin(), m_grid.getZMin(), m_grid.getXMax(), m_grid.getYMax(),
        m_grid.getZMax(), m_grid.getResolutionXY(), m_grid.getResolutionZ());
  }

  /** @name API of the NearestNeighborsCapable virtual interface
    @{ */
  [[nodiscard]] bool nn_has_indices_or_ids() const override { return false; }
  [[nodiscard]] size_t nn_index_count() const override { return 0; }
  [[nodiscard]] bool nn_single_search(
      const mrpt::math::TPoint3Df& query,
      mrpt::math::TPoint3Df& result,
      float& out_dist_sqr,
      uint64_t& resultIndexOrID) const override;
  [[nodiscard]] bool nn_single_search(
      const mrpt::math::TPoint2Df& query,
      mrpt::math::TPoint2Df& result,
      float& out_dist_sqr,
      uint64_t& resultIndexOrID) const override;
  void nn_multiple_search(
      const mrpt::math::TPoint3Df& query,
      const size_t N,
      std::vector<mrpt::math::TPoint3Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs) const override;
  void nn_multiple_search(
      const mrpt::math::TPoint2Df& query,
      const size_t N,
      std::vector<mrpt::math::TPoint2Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs) const override;
  void nn_radius_search(
      const mrpt::math::TPoint3Df& query,
      const float search_radius_sqr,
      std::vector<mrpt::math::TPoint3Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs,
      size_t maxPoints) const override;
  void nn_radius_search(
      const mrpt::math::TPoint2Df& query,
      const float search_radius_sqr,
      std::vector<mrpt::math::TPoint2Df>& results,
      std::vector<float>& out_dists_sqr,
      std::vector<uint64_t>& resultIndicesOrIDs,
      size_t maxPoints) const override;
  /** @} */

 private:
  // See docs in base class
  double internal_computeObservationLikelihood(
      const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const override;
  // See docs in base class
  bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const override;

  MAP_DEFINITION_START(COccupancyGridMap3D)
  /** See COccupancyGridMap3D::COccupancyGridMap3D */
  float min_x{-5.0f}, max_x{5.0f};
  float min_y{-5.0f}, max_y{5.0f};
  float min_z{-5.0f}, max_z{5.0f};
  float resolution{0.25f};

  /** Observations insertion options */
  mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts;
  /** Probabilistic observation likelihood options */
  mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts;
  MAP_DEFINITION_END(COccupancyGridMap3D)
};

}  // namespace mrpt::maps

MRPT_ENUM_TYPE_BEGIN(mrpt::maps::COccupancyGridMap3D::TLikelihoodMethod)
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap3D, lmRayTracing);
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap3D, lmLikelihoodField_Thrun);
MRPT_ENUM_TYPE_END()
