/* +------------------------------------------------------------------------+
   |                     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/maps/CMultiMetricMap.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/rtti/CListOfClasses.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/system/COutputLogger.h>

#include <mutex>

namespace mrpt::slam
{
/** @defgroup metric_slam_grp Metric SLAM algorithms
 * \ingroup mrpt_slam_grp */

/** This virtual class is the base for SLAM implementations. See derived classes
 * for more information.
 *
 * \sa CMetricMap  \ingroup metric_slam_grp
 */
class CMetricMapBuilder : public mrpt::system::COutputLogger
{
 protected:
  /** Critical zones */
  std::mutex critZoneChangingMap;
  /** Enter critical section for map updating */
  inline void enterCriticalSection() { critZoneChangingMap.lock(); }
  /** Leave critical section for map updating */
  inline void leaveCriticalSection() { critZoneChangingMap.unlock(); }

 public:
  /** Constructor */
  CMetricMapBuilder();
  /** Destructor. */
  ~CMetricMapBuilder() override;

  // ---------------------------------------------------------------------
  /** @name Pure virtual methods to implement in any particular SLAM algorithm
    @{ */

  /** Initialize the method, starting with a known location PDF "x0"(if
   * supplied, set to nullptr to left unmodified) and a given fixed, past map.
   */
  virtual void initialize(
      const mrpt::maps::CSimpleMap& initialMap = mrpt::maps::CSimpleMap(),
      const mrpt::poses::CPosePDF* x0 = nullptr) = 0;

  /** Returns a copy of the current best pose estimation as a pose PDF. */
  virtual mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const = 0;

  /** Process a new action and observations pair to update this map: See the
   *description of the class at the top of this page to see a more complete
   *description.
   *  \param action The estimation of the incremental pose change in the robot
   *pose.
   *	\param observations The set of observations that robot senses at the new
   *pose.
   */
  virtual void processActionObservation(
      mrpt::obs::CActionCollection& action, mrpt::obs::CSensoryFrame& observations) = 0;

  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so
   * far built map. */
  virtual void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap& out_map) const = 0;

  /** Returns just how many sensory-frames are stored in the currently build
   * map. */
  virtual unsigned int getCurrentlyBuiltMapSize() = 0;

  /** Returns the map built so far. NOTE that for efficiency a pointer to the
   * internal object is passed, DO NOT delete nor modify the object in any
   * way, if desired, make a copy of ir with "clone()". */
  virtual const mrpt::maps::CMultiMetricMap& getCurrentlyBuiltMetricMap() const = 0;

  /** A useful method for debugging: the current map (and/or poses) estimation
   * is dumped to an image file.
   * \param file The output file name
   * \param formatEMF_BMP Output format = true:EMF, false:BMP
   */
  virtual void saveCurrentEstimationToImage(const std::string& file, bool formatEMF_BMP = true) = 0;

  /** @} */
  // ---------------------------------------------------------------------

  /** Clear all elements of the maps, and reset localization to (0,0,0deg). */
  void clear();

  /** Enables or disables the map updating (default state is enabled) */
  void enableMapUpdating(bool enable) { options.enableMapUpdating = enable; }
  /** Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file */
  void loadCurrentMapFromFile(const std::string& fileName);

  /** Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file. */
  void saveCurrentMapToFile(const std::string& fileName, bool compressGZ = true) const;

  /** Options for the algorithm */
  struct TOptions
  {
    TOptions(mrpt::system::VerbosityLevel& verb_level_ref) : verbosity_level(verb_level_ref) {}

    mrpt::system::VerbosityLevel& verbosity_level;

    /** Enable map updating, default is true. */
    bool enableMapUpdating = true;

    /** Always insert into map. Default is false: detect if necessary. */
    bool debugForceInsertion = false;

    /** A list of observation classes (derived from mrpt::obs::CObservation)
     * which will be always inserted in the map, disregarding the minimum
     * insertion distances).
     *  Default: Empty. How to insert classes:
     *   \code
     *     alwaysInserByClass.insert(CLASS_ID(CObservationImage));
     *   \endcode
     * \sa mrpt::rtti::CListOfClasses
     */
    mrpt::rtti::CListOfClasses alwaysInsertByClass;
  };

  TOptions options;

 public:
};  // End of class def.

}  // namespace mrpt::slam
