/* +------------------------------------------------------------------------+
   |                     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/bayes/CParticleFilterCapable.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/poses/CPoseRandomSampler.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/slam/PF_implementations_data.h>

namespace mrpt
{
namespace slam
{
class CMetricMapBuilderRBPF;
}
namespace maps
{
/** Auxiliary class used in mrpt::maps::CMultiMetricMapPDF
 * \ingroup mrpt_slam_grp
 */
class CRBPFParticleData : public mrpt::serialization::CSerializable
{
  DEFINE_SERIALIZABLE(CRBPFParticleData, mrpt::maps)
 public:
  CRBPFParticleData() = default;
  CRBPFParticleData(const TSetOfMetricMapInitializers& mapsInit) : mapTillNow(mapsInit), robotPath()
  {
  }

  CMultiMetricMap mapTillNow;
  std::deque<mrpt::math::TPose3D> robotPath;
};

/** Declares a class that represents a Rao-Blackwellized set of particles for
 * solving the SLAM problem (This class is the base of RBPF-SLAM applications).
 *   This class is used internally by the map building algorithm in
 * "mrpt::slam::CMetricMapBuilderRBPF"
 *
 * \sa mrpt::slam::CMetricMapBuilderRBPF
 * \ingroup metric_slam_grp
 */
class CMultiMetricMapPDF :
    public mrpt::serialization::CSerializable,
    public mrpt::bayes::CParticleFilterData<CRBPFParticleData>,
    public mrpt::bayes::CParticleFilterDataImpl<
        CMultiMetricMapPDF,
        mrpt::bayes::CParticleFilterData<CRBPFParticleData>::CParticleList>,
    public mrpt::slam::PF_implementation<
        CRBPFParticleData,
        CMultiMetricMapPDF,
        mrpt::bayes::particle_storage_mode::POINTER>
{
  friend class mrpt::slam::CMetricMapBuilderRBPF;

  DEFINE_SERIALIZABLE(CMultiMetricMapPDF, mrpt::maps)

 protected:
  // PF algorithm implementations:
  void prediction_and_update_pfStandardProposal(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;
  void prediction_and_update_pfOptimalProposal(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;
  void prediction_and_update_pfAuxiliaryPFOptimal(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;
  void prediction_and_update_pfAuxiliaryPFStandard(
      const mrpt::obs::CActionCollection* action,
      const mrpt::obs::CSensoryFrame* observation,
      const bayes::CParticleFilter::TParticleFilterOptions& PF_options) override;

 private:
  /** Internal buffer for the averaged map. */
  mrpt::maps::CMultiMetricMap averageMap;
  bool averageMapIsUpdated{false};

  /** The SFs and their corresponding pose estimations */
  mrpt::maps::CSimpleMap SFs;
  /** A mapping between indexes in the SFs to indexes in the robot paths from
   * particles. */
  std::vector<uint32_t> SF2robotPath;

 public:
  /** The struct for passing extra simulation parameters to the
   * prediction/update stage
   *    when running a particle filter.
   * \sa prediction_and_update
   */
  struct TPredictionParams : public mrpt::config::CLoadableOptions
  {
    /** Default settings method */
    TPredictionParams() = 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

    /** [pf optimal proposal only]  Only for PF algorithm=2 (Exact
     * "pfOptimalProposal")
     *   Select the map on which to calculate the optimal
     *    proposal distribution. Values:
     *   0: Gridmap   -> Uses Scan matching-based approximation (based on
     * Stachniss' work)
     *   1: Landmarks -> Uses matching to approximate optimal
     *   2: Beacons   -> Used for exact optimal proposal in RO-SLAM
     *   3: Pointsmap -> Uses Scan matching-based approximation with a map
     * of points (based on Stachniss' work)
     *  Default = 0
     */
    int pfOptimalProposal_mapSelection{0};

    /** [prediction stage][pf optimal proposal only] If
     * useICPGlobalAlign_withGrid=true, this is the minimum quality ratio
     * [0,1] of the alignment such as it will be accepted. Otherwise, raw
     * odometry is used for those bad cases (default=0.7).
     */
    float ICPGlobalAlign_MinQuality{0.70f};

    mrpt::slam::TKLDParams KLD_params;

    /** ICP parameters, used only when "PF_algorithm=2" in the particle
     * filter. */
    mrpt::slam::CICP::TConfigParams icp_params;

  } options;

  /** Constructor */
  CMultiMetricMapPDF() = default;

  CMultiMetricMapPDF(
      const bayes::CParticleFilter::TParticleFilterOptions& opts,
      const mrpt::maps::TSetOfMetricMapInitializers& mapsInitializers,
      const TPredictionParams& predictionOptions);

  /** Clear all elements of the maps, and restore all paths to a single
   * starting pose */
  void clear(const mrpt::poses::CPose2D& initialPose);
  /** \overload */
  void clear(const mrpt::poses::CPose3D& initialPose);
  /** Resets the map by loading an already-mapped map for past poses.
   * Current robot pose should be normally set to the last keyframe
   * in the simplemap. */
  void clear(const mrpt::maps::CSimpleMap& prevMap, const mrpt::poses::CPose3D& currentPose);

  /** Returns the estimate of the robot pose as a particles PDF for the
   * instant of time "timeStep", from 0 to N-1.
   * \sa getEstimatedPosePDF
   */
  void getEstimatedPosePDFAtTime(
      size_t timeStep, mrpt::poses::CPose3DPDFParticles& out_estimation) const;

  /** Returns the current estimate of the robot pose, as a particles PDF.
   * \sa getEstimatedPosePDFAtTime
   */
  void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles& out_estimation) const;

  /** Returns the weighted averaged map based on the current best estimation.
   * If you need a persistent copy of this object, please use
   * "CSerializable::duplicate" and use the copy.
   * \sa Almost 100% sure you would prefer the best current map, given by
   * getCurrentMostLikelyMetricMap()
   */
  const CMultiMetricMap* getAveragedMetricMapEstimation();

  /** Returns a pointer to the current most likely map (associated to the most
   * likely particle) */
  const CMultiMetricMap* getCurrentMostLikelyMetricMap() const;

  /** Get the number of CSensoryFrame inserted into the internal member SFs */
  size_t getNumberOfObservationsInSimplemap() const { return SFs.size(); }
  /** Insert an observation to the map, at each particle's pose and to each
   * particle's metric map.
   * \param sf The SF to be inserted
   * \return true if any may was updated, false otherwise
   */
  bool insertObservation(mrpt::obs::CSensoryFrame& sf);

  /** Return the path (in absolute coordinate poses) for the i'th particle.
   * \exception On index out of bounds
   */
  void getPath(size_t i, std::deque<math::TPose3D>& out_path) const;

  /** Returns the current entropy of paths, computed as the average entropy of
   * poses along the path, where entropy of each pose estimation is computed
   * as the entropy of the gaussian approximation covariance.
   */
  double getCurrentEntropyOfPaths();

  /** Returns the joint entropy estimation over paths and maps, according to
   * "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti
   * and W.Burgard.
   */
  double getCurrentJointEntropy();

  /** Update the poses estimation of the member "SFs" according to the current
   * path belief.
   */
  void updateSensoryFrameSequence();

  /** A logging utility: saves the current path estimation for each particle
   * in a text file (a row per particle, each 3-column-entry is a set
   * [x,y,phi], respectively).
   */
  void saveCurrentPathEstimationToTextFile(const std::string& fil);

 private:
  /** Rebuild the "expected" grid map. Used internally, do not call  */
  void rebuildAverageMap();

  /** An index [0,1] measuring how much information an observation aports to
   * the map (Typ. threshold=0.07) */
  float newInfoIndex{0};

 public:
  /** \name Virtual methods that the PF_implementations assume exist.
    @{ */

  // see docs in base
  mrpt::math::TPose3D getLastPose(size_t i, bool& pose_is_valid) const override;

  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
      CParticleDataContent* particleData, const mrpt::math::TPose3D& newPose) const override;

  // The base implementation is fine for this class.
  // void PF_SLAM_implementation_replaceByNewParticleSet( ...

  bool PF_SLAM_implementation_doWeHaveValidObservations(
      const CParticleList& particles, const mrpt::obs::CSensoryFrame* sf) const override;

  bool PF_SLAM_implementation_skipRobotMovement() const override;

  /** Evaluate the observation likelihood for one particle at a given location
   */
  double PF_SLAM_computeObservationLikelihoodForParticle(
      const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
      size_t particleIndexForMap,
      const mrpt::obs::CSensoryFrame& observation,
      const mrpt::poses::CPose3D& x) const override;
  /** @} */

};  // End of class def.

}  // namespace maps
}  // namespace mrpt
