/* +------------------------------------------------------------------------+
   |                     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/opengl/opengl_frwds.h>
#include <mrpt/serialization/serialization_frwds.h>

#include <cstdint>
#include <map>
#include <vector>

namespace mrpt::nav
{
/** Clearance information for one particular PTG and one set of obstacles.
 * Usage:
 * - Declare an object of this type (it will be initialized to "empty"),
 * - Call CParameterizedTrajectoryGenerator::initClearanceDiagram()
 * - Repeatedly call CParameterizedTrajectoryGenerator::updateClearance() for
 * each 2D obstacle point.
 *
 *  \ingroup nav_tpspace
 */
class ClearanceDiagram
{
   public:
	/** default ctor */
	ClearanceDiagram();
	/** Reset to default, empty state */
	void clear();
	/** Initializes the container to allocate `decimated_num_paths` entries, as
	 * a decimated
	 * subset of a total of `actual_num_paths` paths */
	void resize(size_t actual_num_paths, size_t decimated_num_paths);
	inline bool empty() const { return m_raw_clearances.empty(); }
	inline size_t get_actual_num_paths() const { return m_actual_num_paths; }
	inline size_t get_decimated_num_paths() const
	{
		return m_raw_clearances.size();
	}

	/** Gets the clearance for path `k` and distance `TPS_query_distance` in one
	 * of two modes:
	 * - [integrate_over_path=false] clearance from that specific spot, or
	 * - [integrate_over_path=true] average clearance over the path from the
	 * origin to that specific spot.
	 */
	double getClearance(
		uint16_t k, double TPS_query_distance, bool integrate_over_path) const;
	void renderAs3DObject(
		mrpt::opengl::CMesh& mesh, double min_x, double max_x, double min_y,
		double max_y, double cell_res, bool integrate_over_path) const;

	void readFromStream(mrpt::serialization::CArchive& in);
	void writeToStream(mrpt::serialization::CArchive& out) const;

	/** [TPS_distance] => normalized_clearance_for_exactly_that_robot_pose  */
	using dist2clearance_t = std::map<double, double>;
	dist2clearance_t& get_path_clearance(size_t actual_k);
	const dist2clearance_t& get_path_clearance(size_t actual_k) const;

	inline dist2clearance_t& get_path_clearance_decimated(size_t decim_k)
	{
		return m_raw_clearances[decim_k];
	}
	inline const dist2clearance_t& get_path_clearance_decimated(
		size_t decim_k) const
	{
		return m_raw_clearances[decim_k];
	}

	size_t real_k_to_decimated_k(size_t k) const;
	size_t decimated_k_to_real_k(size_t k) const;

   protected:
	/** Container: [decimated_path_k][TPS_distance] =>
	 * normalized_clearance_for_exactly_that_robot_pose  */
	std::vector<dist2clearance_t> m_raw_clearances;

	size_t m_actual_num_paths{
		0};	 // The decimated number of paths is implicit in
	// raw_clearances.size()
	double m_k_a2d{.0}, m_k_d2a{.0};
};

}  // namespace mrpt::nav
