/* +------------------------------------------------------------------------+
   |                     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/math/TLine3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/poses/CPose3D.h>

namespace mrpt::maps
{
/** Virtual base class for Digital Elevation Model (DEM) maps. See derived
 * classes for details.
 * This class implements those operations which are especific to DEMs.
 * \ingroup mrpt_maps_grp */
class CHeightGridMap2D_Base
{
   public:
	CHeightGridMap2D_Base();
	virtual ~CHeightGridMap2D_Base();

	/** @name Specific API for Digital Elevation Model (DEM) maps
		@{ */
	/** Gets the intersection between a 3D line and a Height Grid map (taking
	 * into account the different heights of each individual cell)  */
	bool intersectLine3D(
		const mrpt::math::TLine3D& r1, mrpt::math::TObject3D& obj) const;

	/** Computes the minimum and maximum height in the grid.
	 * \return False if there is no observed cell yet. */
	bool getMinMaxHeight(float& z_min, float& z_max) const;

	/** Extra params for insertIndividualPoint() */
	struct TPointInsertParams
	{
		/** (Default:0.0) If !=0, use this value as the uncertainty (standard
		 * deviation) for the point "z" coordinate, instead of the map-wise
		 * default value. */
		double pt_z_std{0.0};
		/** (default: true) run any required operation to ensure the map
		 * reflects the changes caused by this point. Otherwise, calling
		 * dem_update_map() is required. */
		bool update_map_after_insertion{true};

		TPointInsertParams();
	};
	/** Update the DEM with one new point.
	 * \sa mrpt::maps::CMetricMap::insertObservation() for inserting
	 * higher-level objects like 2D/3D LIDAR scans
	 * \return true if updated OK, false if (x,y) is out of bounds */
	virtual bool insertIndividualPoint(
		const double x, const double y, const double z,
		const TPointInsertParams& params = TPointInsertParams()) = 0;

	virtual double dem_get_resolution() const = 0;
	virtual size_t dem_get_size_x() const = 0;
	virtual size_t dem_get_size_y() const = 0;
	/** Get cell 'z' by (cx,cy) cell indices. \return false if out of bounds or
	 * un-observed cell. */
	virtual bool dem_get_z_by_cell(
		const size_t cx, const size_t cy, double& z_out) const = 0;
	/** Get cell 'z' (x,y) by metric coordinates. \return false if out of bounds
	 * or un-observed cell. */
	virtual bool dem_get_z(
		const double x, const double y, double& z_out) const = 0;
	/** Ensure that all observations are reflected in the map estimate */
	virtual void dem_update_map() = 0;
	/** @} */

	/** Internal method called by internal_insertObservation() */
	bool dem_internal_insertObservation(
		const mrpt::obs::CObservation& obs,
		const std::optional<const mrpt::poses::CPose3D>& robotPose =
			std::nullopt);
};
}  // namespace mrpt::maps
