/* +------------------------------------------------------------------------+
   |                     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/poses/CRobot2DPoseEstimator.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/slam/CMetricMapBuilder.h>

#include <map>

namespace mrpt::slam
{
/** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic
 *pose tracking algorithm.
 *   Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap"
 *objects. The methods are
 *	 thread-safe.
 * \ingroup metric_slam_grp
 */
class CMetricMapBuilderICP : public mrpt::slam::CMetricMapBuilder
{
 public:
  /** Default constructor - Upon construction, you can set the parameters in
   * ICP_options, then call "initialize".
   */
  CMetricMapBuilderICP();

  /** Destructor:
   */
  ~CMetricMapBuilderICP() override;

  /** Algorithm configuration params
   */
  struct TConfigParams : public mrpt::config::CLoadableOptions
  {
    /** Initializer */
    TConfigParams(mrpt::system::VerbosityLevel& parent_verbosity_level);
    TConfigParams& operator=(const TConfigParams& other);  // Copy
    TConfigParams(const TConfigParams& other) : verbosity_level(other.verbosity_level)
    {
      *this = other;
    }

    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
    /** (default:false) Match against the occupancy grid or the points map?
     * The former is quicker but less precise. */
    bool matchAgainstTheGrid;

    /** Minimum robot linear (m) displacement for a new observation to be
     * inserted in the map. */
    double insertionLinDistance;
    /** Minimum robot angular (rad, deg when loaded from the .ini)
     * displacement for a new observation to be inserted in the map. */
    double insertionAngDistance;
    /** Minimum robot linear (m) displacement for a new observation to be
     * used to do ICP-based localization (otherwise, dead-reckon with
     * odometry). */
    double localizationLinDistance;
    /** Minimum robot angular (rad, deg when loaded from the .ini)
     * displacement for a new observation to be used to do ICP-based
     * localization (otherwise, dead-reckon with odometry). */
    double localizationAngDistance;

    /** Minimum ICP goodness (0,1) to accept the resulting corrected
     * position (default: 0.40) */
    double minICPgoodnessToAccept;

    mrpt::system::VerbosityLevel& verbosity_level;

    /** What maps to create (at least one points map and/or a grid map are
     * needed).
     *  For the expected format in the .ini file when loaded with
     * loadFromConfigFile(), see documentation of
     * TSetOfMetricMapInitializers.
     */
    mrpt::maps::TSetOfMetricMapInitializers mapInitializers;
  };

  /** Options for the ICP-SLAM application \sa ICP_params */
  TConfigParams ICP_options;
  /** Options for the ICP algorithm itself \sa ICP_options */
  CICP::TConfigParams ICP_params;

  /** Initialize the method, starting with a known location PDF "x0"(if
   * supplied, set to nullptr to left unmodified) and a given fixed, past map.
   *  This method MUST be called if using the default constructor, after
   * loading the configuration into ICP_options. In particular,
   * TConfigParams::mapInitializers
   */
  void initialize(
      const mrpt::maps::CSimpleMap& initialMap = mrpt::maps::CSimpleMap(),
      const mrpt::poses::CPosePDF* x0 = nullptr) override;

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

  /** Sets the "current map file", thus that map will be loaded if it exists
   * or a new one will be created if it does not, and the updated map will be
   * save to that file when destroying the object.
   */
  void setCurrentMapFile(const char* mapFile);

  /** Appends a new action and observations 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 in_SF The set of observations that robot senses at the new pose.
   * See params in CMetricMapBuilder::options and
   *CMetricMapBuilderICP::ICP_options
   * \sa processObservation
   */
  void processActionObservation(
      mrpt::obs::CActionCollection& action, mrpt::obs::CSensoryFrame& in_SF) override;

  /**  The main method of this class: Process one odometry or sensor
   observation.
    The new entry point of the algorithm (the old one  was
   processActionObservation, which now is a wrapper to
  this method).
   * See params in CMetricMapBuilder::options and
   CMetricMapBuilderICP::ICP_options
  */
  void processObservation(const mrpt::obs::CObservation::Ptr& obs);

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

  /** Returns the 2D points of current local map */
  void getCurrentMapPoints(std::vector<float>& x, std::vector<float>& y);

  const mrpt::maps::CMultiMetricMap& getCurrentlyBuiltMetricMap() const override;

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

  /** 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
   */
  void saveCurrentEstimationToImage(const std::string& file, bool formatEMF_BMP = true) override;

 private:
  /** The set of observations that leads to current map: */
  mrpt::maps::CSimpleMap SF_Poses_seq;

  /** The metric map representation as a points map: */
  mrpt::maps::CMultiMetricMap metricMap;

  /** Current map file. */
  std::string currentMapFile;

  /** The pose estimation by the alignment algorithm (ICP). */
  /** Last pose estimation (Mean) */
  mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst;
  /** Last pose estimation (covariance) */
  mrpt::math::CMatrixDouble33 m_lastPoseEst_cov;

  /** The estimated robot path:
   */
  std::deque<mrpt::math::TPose2D> m_estRobotPath;
  mrpt::poses::CPose2D m_auxAccumOdometry;

  /** Traveled distances from last map update / ICP-based localization. */
  struct TDist
  {
    TDist() = default;
    double lin{0};  // meters
    double ang{0};  // degrees
    mrpt::math::TPose2D last_update;

    void updateDistances(const mrpt::poses::CPose2D& p);
    void updatePose(const mrpt::poses::CPose2D& p);
  };
  TDist m_distSinceLastICP;
  /** Indexed by sensor label. */
  std::map<std::string, TDist> m_distSinceLastInsertion;
  bool m_there_has_been_an_odometry{false};

  void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D& new_pose);
  void resetRobotDisplacementCounters(const mrpt::poses::CPose2D& new_pose);
};

}  // namespace mrpt::slam
