/* +------------------------------------------------------------------------+
   |                     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/CConfigFileBase.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/slam/CICP.h>

namespace mrpt::graphslam::deciders
{
/**\brief Class for keeping together all the RangeScanner-related functions.
 *
 * ## Description
 *
 * Deciders that make use of either 2DRangeScans (laser generated
 * observations) or 3DRangeScans (RGBD-cameras) can inherit from
 * this class in case they want to use the underlying methods
 *
 * ### .ini Configuration Parameters
 *
 * \htmlinclude graphslam-engine_config_params_preamble.txt
 *
 * - \b conversion_sensor_label
 *   + \a Default value : "KINECT_TO_2D_SCAN"
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_angle_sup
 *   + \a Default value : 10
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_angle_inf
 *   + \a Default value : 10
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_oversampling_ratio
 *   + \a Default value : 1.1
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * \note Class contains an instance of the mrpt::slam::CICP class and it parses
 * the configuration parameters of the latter from the
 * "ICP" section. Refer to
 * mrpt::slam::CICP documentation for its list of
 * configuration parameters
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CRangeScanOps
{
  using constraint_t = typename GRAPH_T::constraint_t;
  using self_t = CRangeScanOps<GRAPH_T>;

 protected:
  /**\brief Align the 2D range scans provided and fill the potential edge that
   * can transform the one into the other.
   *
   * User can optionally ask that additional information be returned in a
   * TReturnInfo struct
   */
  void getICPEdge(
      const mrpt::obs::CObservation2DRangeScan& from,
      const mrpt::obs::CObservation2DRangeScan& to,
      constraint_t* rel_edge,
      const mrpt::poses::CPose2D* initial_pose = nullptr,
      mrpt::slam::CICP::TReturnInfo* icp_info = nullptr);
  /**\brief Align the 3D range scans provided and find the potential edge that
   * can transform the one into the other.
   *
   * Fills the 2D part (rel_edge) of the 3D constraint between the scans,
   * since
   * we are interested in computing the 2D alignment. User can optionally ask
   * that additional information be returned in a TReturnInfo struct
   */
  void getICPEdge(
      const mrpt::obs::CObservation3DRangeScan& from,
      const mrpt::obs::CObservation3DRangeScan& to,
      constraint_t* rel_edge,
      const mrpt::poses::CPose2D* initial_pose = nullptr,
      mrpt::slam::CICP::TReturnInfo* icp_info = nullptr);
  /**\brief Reduce the size of the given CPointsMap by keeping one out of
   * "keep_point_every" points.
   *
   * \note If low_lim is set then the PointsMap will contain at least low_lim
   * measurements, regardless of keep_point_every value. Set low_lim to 0 if
   * no
   * lower limit is to be specified
   */
  void decimatePointsMap(
      mrpt::maps::CPointsMap* m, size_t keep_point_every = 4, size_t low_lim = 0);
  /**\brief Wrapper around the CObservation3DRangeScan::convertTo2DScan
   * corresponding method
   *
   * \return True if operation was successful, false otherwise
   */
  bool convert3DTo2DRangeScan(
      mrpt::obs::CObservation3DRangeScan::Ptr& scan3D_in,
      mrpt::obs::CObservation2DRangeScan::Ptr* scan2D_out);

  struct TParams : public mrpt::config::CLoadableOptions
  {
   public:
    TParams() = default;
    ~TParams() override = default;

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

    mrpt::slam::CICP icp;

    /**\brief Struct holding the parameters of 3D to the corresponding 2D
     * range scan conversion.
     */
    mrpt::obs::T3DPointsTo2DScanParams conversion_params;

    bool has_read_config = false;
  };
  TParams params;
};
}  // namespace mrpt::graphslam::deciders
#include "CRangeScanOps_impl.h"
