/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/containers/CDynamicGrid3D.h>
#include <mrpt/maps/CLogOddsGridMapLUT.h>
#include <mrpt/maps/logoddscell_traits.h>

namespace mrpt::maps
{
/** A generic provider of log-odds grid-map maintainance functions.
 *  Map cells must be type TCELL, which can be only:
 *		- int8_t or
 *		- int16_t
 *
 *  \sa CLogOddsGridMapLUT, See derived classes for usage examples.
 * \ingroup mrpt_maps_grp
 */
template <typename TCELL>
struct CLogOddsGridMap3D : public detail::logoddscell_traits<TCELL>
{
	/** The type of cells */
	using cell_t = TCELL;
	using traits_t = detail::logoddscell_traits<TCELL>;
	using grid_t = mrpt::containers::CDynamicGrid3D<TCELL>;

	/** The actual 3D voxels container */
	grid_t m_grid;

	/** Performs Bayesian fusion of a new observation of a cell.
	 * This method increases the "occupancy-ness" of a cell, managing possible
	 * saturation.
	 *  \param theCell The cell to modify
	 *  \param logodd_obs Observation of the cell, in log-odd form as
	 * transformed by p2l.
	 *  \param thres  This must be CELLTYPE_MIN+logodd_obs
	 * \sa updateCell, updateCell_fast_free
	 */
	inline void updateCell_fast_occupied(
		cell_t* theCell, const cell_t logodd_obs, const cell_t thres)
	{
		if (theCell == nullptr) return;
		if (*theCell > thres) *theCell -= logodd_obs;
		else
			*theCell = traits_t::CELLTYPE_MIN;
	}

	/** Performs Bayesian fusion of a new observation of a cell.
	 * This method increases the "occupancy-ness" of a cell, managing possible
	 * saturation.
	 *  \param x Cell index in X axis.
	 *  \param y Cell index in Y axis.
	 *  \param z Cell index in Z axis.
	 *  \param logodd_obs Observation of the cell, in log-odd form as
	 * transformed by p2l.
	 *  \param thres  This must be CELLTYPE_MIN+logodd_obs
	 * \sa updateCell, updateCell_fast_free
	 */
	inline void updateCell_fast_occupied(
		const unsigned x, const unsigned y, const unsigned z,
		const cell_t logodd_obs, const cell_t thres)
	{
		updateCell_fast_occupied(
			m_grid.cellByIndex(x, y, z), logodd_obs, thres);
	}

	/** Performs Bayesian fusion of a new observation of a cell.
	 * This method increases the "free-ness" of a cell, managing possible
	 * saturation.
	 *  \param logodd_obs Observation of the cell, in log-odd form as
	 * transformed by p2l.
	 *  \param thres  This must be CELLTYPE_MAX-logodd_obs
	 * \sa updateCell_fast_occupied
	 */
	inline void updateCell_fast_free(
		cell_t* theCell, const cell_t logodd_obs, const cell_t thres)
	{
		if (theCell == nullptr) return;
		if (*theCell < thres) *theCell += logodd_obs;
		else
			*theCell = traits_t::CELLTYPE_MAX;
	}

	/** Performs the Bayesian fusion of a new observation of a cell.
	 * This method increases the "free-ness" of a cell, managing possible
	 * saturation.
	 *  \param x Cell index in X axis.
	 *  \param y Cell index in Y axis.
	 *  \param z Cell index in Z axis.
	 *  \param logodd_obs Observation of the cell, in log-odd form as
	 * transformed by p2l.
	 *  \param thres  This must be CELLTYPE_MAX-logodd_obs
	 * \sa updateCell_fast_occupied
	 */
	inline void updateCell_fast_free(
		const unsigned x, const unsigned y, const unsigned z,
		const cell_t logodd_obs, const cell_t thres)
	{
		updateCell_fast_free(m_grid.cellByIndex(x, y, z), logodd_obs, thres);
	}
};

}  // namespace mrpt::maps
