/* +------------------------------------------------------------------------+
   |                     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/CLoadableOptions.h>
#include <mrpt/containers/CDynamicGrid.h>
#include <mrpt/img/CImage.h>
#include <mrpt/maps/CLogOddsGridMap2D.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/serialization/CSerializable.h>

namespace mrpt::maps
{
/** A 2D grid map representing the reflectivity of the environment (for example,
 *measured with an IR proximity sensor).
 *
 *  Important implemented features are:
 *		- Insertion of mrpt::obs::CObservationReflectivity observations.
 *		- Probability estimation of observations. See base class.
 *		- Rendering as 3D object: a 2D textured plane.
 *		- Automatic resizing of the map limits when inserting observations close
 *to
 *the border.
 *
 *   Each cell contains the up-to-date average height from measured falling in
 *that cell. Algorithms that can be used:
 *		- mrSimpleAverage: Each cell only stores the current average value.
 * \ingroup mrpt_maps_grp
 */
class CReflectivityGridMap2D : public CMetricMap,
							   public mrpt::containers::CDynamicGrid<int8_t>,
							   public CLogOddsGridMap2D<int8_t>
{
	DEFINE_SERIALIZABLE(CReflectivityGridMap2D, mrpt::maps)

   protected:
	/** Lookup tables for log-odds */
	static CLogOddsGridMapLUT<cell_t> m_logodd_lut;

   public:
	/** Calls the base CMetricMap::clear
	 * Declared here to avoid ambiguity between the two clear() in both base
	 * classes.
	 */
	inline void clear() { CMetricMap::clear(); }
	float cell2float(const int8_t& c) const override
	{
		return m_logodd_lut.l2p(c);
	}

	/** Constructor  */
	CReflectivityGridMap2D(
		double x_min = -2, double x_max = 2, double y_min = -2,
		double y_max = 2, double resolution = 0.1);

	/** Returns true if the map is empty/no observation has been inserted. */
	bool isEmpty() const override;

	/** Parameters related with inserting observations into the map.
	 */
	struct TInsertionOptions : public mrpt::config::CLoadableOptions
	{
		/** Default values loader */
		TInsertionOptions();

		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

		int16_t channel{-1};  //!< The reflectivity channel for this map. If
		//! channel=-1, then any channel will be accepted.
		//! Otherwise, the map will ignore
		//! CObservationReflectivity instances with a differing
		//! channel. (Default=-1)
	} insertionOptions;

	/** See docs in base class: in this class this always returns 0 */
	float compute3DMatchingRatio(
		const mrpt::maps::CMetricMap* otherMap,
		const mrpt::poses::CPose3D& otherMapPose,
		const TMatchingRatioParams& params) const override;

	void saveMetricMapRepresentationToFile(
		const std::string& filNamePrefix) const override;

	void getVisualizationInto(
		mrpt::opengl::CSetOfObjects& outObj) const override;

	/** Returns the grid as a 8-bit graylevel image, where each pixel is a cell
	 * (output image is RGB only if forceRGB is true) */
	void getAsImage(
		mrpt::img::CImage& img, bool verticalFlip = false,
		bool forceRGB = false) const;

	/** Returns a short description of the map. */
	std::string asString() const override
	{
		return "ReflectivityGridMap2D map";
	}

   protected:
	// See docs in base class
	void internal_clear() override;
	bool internal_insertObservation(
		const mrpt::obs::CObservation& obs,
		const std::optional<const mrpt::poses::CPose3D>& robotPose =
			std::nullopt) override;
	double internal_computeObservationLikelihood(
		const mrpt::obs::CObservation& obs,
		const mrpt::poses::CPose3D& takenFrom) const override;

	MAP_DEFINITION_START(CReflectivityGridMap2D)
	/** See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions */
	double min_x{-10.0f}, max_x{10.0f}, min_y{-10.0f}, max_y{10.0f},
		resolution{0.10f};
	mrpt::maps::CReflectivityGridMap2D::TInsertionOptions insertionOpts;
	MAP_DEFINITION_END(CReflectivityGridMap2D)
};

}  // namespace mrpt::maps
