/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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"
