/* +------------------------------------------------------------------------+
   |                     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/CPointCloudFilterBase.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/poses/CPose3D.h>

#include <map>

namespace mrpt::maps
{
/** Implementation of pointcloud filtering based on requisities for minimum
 * neigbouring points in both,
 *   the current timestamp and a previous one.
 *
 * \sa CPointsMap
 * \ingroup mrpt_maps_grp
 */
class CPointCloudFilterByDistance : public mrpt::maps::CPointCloudFilterBase
{
 public:
  // See base docs
  void filter(
      /** [in,out] The input pointcloud, which will be modified upon
       return after filtering. */
      mrpt::maps::CPointsMap* inout_pointcloud,
      /** [in] The timestamp of the input pointcloud */
      const mrpt::system::TTimeStamp pc_timestamp,
      /** [in] If nullptr, the PC is assumed to be given in global
       coordinates. Otherwise, it will be transformed from local
       coordinates to global using this transformation. */
      const mrpt::poses::CPose3D& pc_reference_pose,
      /** [in,out] additional in/out parameters */
      TExtraFilterParams* params = nullptr) override;

  struct TOptions : public mrpt::config::CLoadableOptions
  {
    /** (Default: 0.05 m) */
    double min_dist{0.10};
    /** (Default: 2 deg) Stored in rad. */
    double angle_tolerance;
    /** (Default: 1 s) */
    double too_old_seconds{1.0};
    /** (Default: 1) How many previous keyframes will be compared with the
     * latest pointcloud. */
    int previous_keyframes{1};
    /** (Default: 0.4) If the ratio [0,1] of points considered invalid
     * ("deletion") is larger than this ratio, no point will be deleted
     * since it'd be too suspicious and may indicate a failure of this
     * filter. */
    double max_deletion_ratio{.4};

    TOptions();
    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source,
        const std::string& section) override;  // See base docs
    void saveToConfigFile(
        mrpt::config::CConfigFileBase& c, const std::string& section) const override;
  };

  TOptions options;

 private:
  struct FrameInfo
  {
    mrpt::poses::CPose3D pose;
    mrpt::maps::CSimplePointsMap::Ptr pc;
  };

  std::map<mrpt::system::TTimeStamp, FrameInfo> m_last_frames;
};
}  // namespace mrpt::maps
