/* +------------------------------------------------------------------------+
   |                     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/core/pimpl.h>
#include <mrpt/core/safe_pointers.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/opengl/COctoMapVoxels.h>
#include <mrpt/opengl/Scene.h>

namespace mrpt::maps
{
/** A three-dimensional probabilistic occupancy grid, implemented as an
 * octo-tree with the "octomap" C++ library.
 *  This base class represents a 3D map where each voxel may contain an
 * "occupancy" property, RGBD data, etc. depending on the derived class.
 *
 * As with any other mrpt::maps::CMetricMap, you can obtain a 3D representation
 * of the map calling getAs3DObject() or getAsOctoMapVoxels()
 *
 * To use octomap's iterators to go through the voxels, use
 * COctoMap::getOctomap()
 *
 * The octomap library was presented in \cite wurm2010octomap
 *
 * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
 * \ingroup mrpt_maps_grp
 */
template <class octree_t, class octree_node_t>
class COctoMapBase : public mrpt::maps::CMetricMap
{
 public:
  using myself_t = COctoMapBase<octree_t, octree_node_t>;

  /** Constructor, defines the resolution of the octomap (length of each voxel
   * side) */
  COctoMapBase(double resolution);
  ~COctoMapBase() override = default;
  /** Get a reference to the internal octomap object. Example:
   * \code
   *  mrpt::maps::COctoMap  map;
   *  octomap::OcTree &om = map.getOctomap<octomap::OcTree>();
   * \endcode
   */
  template <class OCTOMAP_CLASS>
  inline OCTOMAP_CLASS& getOctomap()
  {
    return m_impl->m_octomap;
  }

  /** Returns a short description of the map. */
  std::string asString() const override { return "Octomap"; }

  /** With this struct options are provided to the observation insertion
   * process.
   * \sa CObservation::insertObservationInto()
   */
  struct TInsertionOptions : public mrpt::config::CLoadableOptions
  {
    /** Initialization of default parameters */
    TInsertionOptions(myself_t& parent);

    /** Special constructor, not attached to a real COctoMap object: used
     * only in limited situations, since get*() methods don't work, etc. */
    TInsertionOptions();
    TInsertionOptions(const TInsertionOptions& o) { this->operator=(o); }

    TInsertionOptions& operator=(const TInsertionOptions& o)
    {
      // Copy all but the m_parent pointer!
      maxrange = o.maxrange;
      pruning = o.pruning;
      const bool o_has_parent = o.m_parent.get() != nullptr;
      setOccupancyThres(o_has_parent ? o.getOccupancyThres() : o.occupancyThres);
      setProbHit(o_has_parent ? o.getProbHit() : o.probHit);
      setProbMiss(o_has_parent ? o.getProbMiss() : o.probMiss);
      setClampingThresMin(o_has_parent ? o.getClampingThresMin() : o.clampingThresMin);
      setClampingThresMax(o_has_parent ? o.getClampingThresMax() : o.clampingThresMax);
      return *this;
    }

    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;                 // See base docs
    void dumpToTextStream(std::ostream& out) const override;  // See base docs

    double maxrange{-1.};  //!< maximum range for how long individual beams are
    //! inserted (default -1: complete beam)
    bool pruning{true};  //!< whether the tree is (losslessly) pruned after
    //! insertion (default: true)

    /// (key name in .ini files: "occupancyThres") sets the threshold for
    /// occupancy (sensor model) (Default=0.5)
    void setOccupancyThres(double prob)
    {
      if (m_parent.get()) m_parent->setOccupancyThres(prob);
    }
    /// (key name in .ini files: "probHit")sets the probablility for a "hit"
    /// (will be converted to logodds) - sensor model (Default=0.7)
    void setProbHit(double prob)
    {
      if (m_parent.get()) m_parent->setProbHit(prob);
    }
    /// (key name in .ini files: "probMiss")sets the probablility for a
    /// "miss" (will be converted to logodds) - sensor model (Default=0.4)
    void setProbMiss(double prob)
    {
      if (m_parent.get()) m_parent->setProbMiss(prob);
    }
    /// (key name in .ini files: "clampingThresMin")sets the minimum
    /// threshold for occupancy clamping (sensor model) (Default=0.1192, -2
    /// in log odds)
    void setClampingThresMin(double thresProb)
    {
      if (m_parent.get()) m_parent->setClampingThresMin(thresProb);
    }
    /// (key name in .ini files: "clampingThresMax")sets the maximum
    /// threshold for occupancy clamping (sensor model) (Default=0.971, 3.5
    /// in log odds)
    void setClampingThresMax(double thresProb)
    {
      if (m_parent.get()) m_parent->setClampingThresMax(thresProb);
    }

    /// @return threshold (probability) for occupancy - sensor model
    double getOccupancyThres() const
    {
      if (m_parent.get())
        return m_parent->getOccupancyThres();
      else
        return this->occupancyThres;
    }
    /// @return threshold (logodds) for occupancy - sensor model
    float getOccupancyThresLog() const { return m_parent->getOccupancyThresLog(); }

    /// @return probablility for a "hit" in the sensor model (probability)
    double getProbHit() const
    {
      if (m_parent.get())
        return m_parent->getProbHit();
      else
        return this->probHit;
    }
    /// @return probablility for a "hit" in the sensor model (logodds)
    float getProbHitLog() const { return m_parent->getProbHitLog(); }
    /// @return probablility for a "miss"  in the sensor model (probability)
    double getProbMiss() const
    {
      if (m_parent.get())
        return m_parent->getProbMiss();
      else
        return this->probMiss;
    }
    /// @return probablility for a "miss"  in the sensor model (logodds)
    float getProbMissLog() const { return m_parent->getProbMissLog(); }
    /// @return minimum threshold for occupancy clamping in the sensor model
    /// (probability)
    double getClampingThresMin() const
    {
      if (m_parent.get())
        return m_parent->getClampingThresMin();
      else
        return this->clampingThresMin;
    }
    /// @return minimum threshold for occupancy clamping in the sensor model
    /// (logodds)
    float getClampingThresMinLog() const { return m_parent->getClampingThresMinLog(); }
    /// @return maximum threshold for occupancy clamping in the sensor model
    /// (probability)
    double getClampingThresMax() const
    {
      if (m_parent.get())
        return m_parent->getClampingThresMax();
      else
        return this->clampingThresMax;
    }
    /// @return maximum threshold for occupancy clamping in the sensor model
    /// (logodds)
    float getClampingThresMaxLog() const { return m_parent->getClampingThresMaxLog(); }

   private:
    mrpt::ignored_copy_ptr<myself_t> m_parent;

    double occupancyThres{0.5};  // sets the threshold for occupancy (sensor
    // model) (Default=0.5)
    double probHit{0.7};  // sets the probablility for a "hit" (will be converted
    // to logodds) - sensor model (Default=0.7)
    double probMiss{0.4};  // sets the probablility for a "miss" (will be
    // converted to logodds) - sensor model (Default=0.4)
    double clampingThresMin{0.1192};  // sets the minimum threshold for occupancy
    // clamping (sensor model) (Default=0.1192, -2
    // in log odds)
    double clampingThresMax{0.971};  // sets the maximum threshold for occupancy
                                     // clamping (sensor model) (Default=0.971, 3.5
                                     // in log odds)
  };

  TInsertionOptions insertionOptions;  //!< The options used when inserting
  //! observations in the map

  /** Options used when evaluating "computeObservationLikelihood"
   * \sa CObservation::computeObservationLikelihood
   */
  struct TLikelihoodOptions : public mrpt::config::CLoadableOptions
  {
    /** Initialization of default parameters
     */
    TLikelihoodOptions();
    ~TLikelihoodOptions() override = default;
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;                 // See base docs
    void dumpToTextStream(std::ostream& out) const override;  // See base docs

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

    uint32_t decimation{1};  //!< Speed up the likelihood computation by
                             //! considering only one out of N rays (default=1)
  };

  TLikelihoodOptions likelihoodOptions;

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

  /** Options for the conversion of a mrpt::maps::COctoMap into a
   * mrpt::opengl::COctoMapVoxels */
  struct TRenderingOptions
  {
    bool generateGridLines{false};  //!< Generate grid lines for all octree nodes,
    //! useful to draw the "structure" of the
    //! octree, but computationally costly (Default:
    //! false)

    bool generateOccupiedVoxels{true};  //!< Generate voxels for the occupied
    //! volumes  (Default=true)
    bool visibleOccupiedVoxels{true};  //!< Set occupied voxels visible (requires
    //! generateOccupiedVoxels=true)
    //!(Default=true)

    bool generateFreeVoxels{true};  //!< Generate voxels for the freespace
    //!(Default=true)
    bool visibleFreeVoxels{true};  //!< Set free voxels visible (requires
    //! generateFreeVoxels=true) (Default=true)

    TRenderingOptions() = default;

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

  TRenderingOptions renderingOptions;

  /** Returns a 3D object representing the map.
   * \sa renderingOptions
   */
  void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const override
  {
    auto gl_obj = mrpt::opengl::COctoMapVoxels::Create();
    this->getAsOctoMapVoxels(*gl_obj);
    o.insert(gl_obj);
  }

  /** Builds a renderizable representation of the octomap as a
   * mrpt::opengl::COctoMapVoxels object.
   * \sa renderingOptions
   */
  virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const = 0;

  /** Get the occupancy probability [0,1] of a point
   * \return false if the point is not mapped, in which case the returned
   * "prob" is undefined. */
  bool getPointOccupancy(const float x, const float y, const float z, double& prob_occupancy) const;

  /** Update the octomap with a 2D or 3D scan, given directly as a point cloud
   * and the 3D location of the sensor (the origin of the rays) in this map's
   * frame of reference.
   * Insertion parameters can be found in \a insertionOptions.
   * \sa The generic observation insertion method
   * CMetricMap::insertObservation()
   */
  void insertPointCloud(
      const CPointsMap& ptMap, const float sensor_x, const float sensor_y, const float sensor_z);

  /** Performs raycasting in 3d, similar to computeRay().
   *
   * A ray is cast from origin with a given direction, the first occupied
   * cell is returned (as center coordinate). If the starting coordinate is
   * already
   * occupied in the tree, this coordinate will be returned as a hit.
   *
   * @param origin starting coordinate of ray
   * @param direction A vector pointing in the direction of the raycast. Does
   * not need to be normalized.
   * @param end returns the center of the cell that was hit by the ray, if
   * successful
   * @param ignoreUnknownCells whether unknown cells are ignored. If false
   * (default), the raycast aborts when an unkown cell is hit.
   * @param maxRange Maximum range after which the raycast is aborted (<= 0:
   * no limit, default)
   * @return whether or not an occupied cell was hit
   */
  bool castRay(
      const mrpt::math::TPoint3D& origin,
      const mrpt::math::TPoint3D& direction,
      mrpt::math::TPoint3D& end,
      bool ignoreUnknownCells = false,
      double maxRange = -1.0) const;

  virtual void setOccupancyThres(double prob) = 0;
  virtual void setProbHit(double prob) = 0;
  virtual void setProbMiss(double prob) = 0;
  virtual void setClampingThresMin(double thresProb) = 0;
  virtual void setClampingThresMax(double thresProb) = 0;
  virtual double getOccupancyThres() const = 0;
  virtual float getOccupancyThresLog() const = 0;
  virtual double getProbHit() const = 0;
  virtual float getProbHitLog() const = 0;
  virtual double getProbMiss() const = 0;
  virtual float getProbMissLog() const = 0;
  virtual double getClampingThresMin() const = 0;
  virtual float getClampingThresMinLog() const = 0;
  virtual double getClampingThresMax() const = 0;
  virtual float getClampingThresMaxLog() const = 0;

 protected:
  /**  Builds the list of 3D points in global coordinates for a generic
   * observation. Used for both, insertObservation() and computeLikelihood().
   * \param[out] point3d_sensorPt Is a pointer to a "point3D".
   * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not
   * declared as such to avoid headers dependencies in user code.
   * \return false if the observation kind is not applicable.
   */
  template <class octomap_point3d, class octomap_pointcloud>
  bool internal_build_PointCloud_for_observation(
      const mrpt::obs::CObservation& obs,
      const std::optional<const mrpt::poses::CPose3D>& robotPose,
      octomap_point3d& sensorPt,
      octomap_pointcloud& scan) const;

  struct Impl;

  mrpt::pimpl<Impl> m_impl;

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

};  // End of class def.
}  // namespace mrpt::maps
