/* +------------------------------------------------------------------------+
   |                     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/maps/COccupancyGridMap2D.h>
#include <mrpt/math/TPoint2D.h>
#include <mrpt/poses/CPose2D.h>

namespace mrpt::nav
{
/** \addtogroup nav_planners Path planning
 * \ingroup mrpt_nav_grp
 * @{ */

/** Searches for collision-free path in 2D occupancy grids for holonomic
 * circular robots.
 *  The implementation first enlargest obstacles with robot radius, then applies
 * a
 *  wavefront algorithm to find the shortest free path between origin and target
 * 2D points.
 *
 * Notice that this simple planner does not take into account robot kinematic
 * constraints.
 */
class PlannerSimple2D
{
   public:
	PlannerSimple2D() = default;
	virtual ~PlannerSimple2D() = default;

	/** The maximum occupancy probability to consider a cell as an obstacle,
	 * default=0.5  */
	float occupancyThreshold{0.5f};

	/** The minimum distance between points in the returned found path
	 * (default=0.4); Notice
	 *  that full grid resolution is used in path finding, this is only a way
	 * to reduce the
	 *  amount of redundant information to be returned.
	 */
	float minStepInReturnedPath{0.4f};

	/** The aproximate robot radius used in the planification. Default is 0.35m
	 */
	float robotRadius{0.35f};

	/** This method compute the optimal path for a circular robot, in the given
	 *   occupancy grid map, from the origin location to a target point.
	 * The options and additional parameters to this method can be set with
	 *   member configuration variables.
	 *
	 * \param theMap	[IN] The occupancy gridmap used to the planning.
	 * \param origin	[IN] The starting pose of the robot, in coordinates of
	 * "map".
	 * \param target	[IN] The desired target pose for the robot, in
	 * coordinates of "map".
	 * \param path		[OUT] The found path, in global coordinates relative
	 * to "map".
	 * \param notFount	[OUT] Will be true if no path has been found.
	 * \param maxSearchPathLength [IN] The maximum path length to search for,
	 * in meters (-1 = no limit)
	 *
	 * \sa robotRadius
	 *
	 * \note If either the origin or the target are out of the gridmap
	 * extensions, `notFound` will be returned as `true`.
	 *
	 * \exception std::exception On any error
	 */
	void computePath(
		const mrpt::maps::COccupancyGridMap2D& theMap,
		const mrpt::poses::CPose2D& origin, const mrpt::poses::CPose2D& target,
		std::deque<mrpt::math::TPoint2D>& path, bool& notFound,
		float maxSearchPathLength = -1) const;
};

/** @} */
}  // namespace mrpt::nav
