/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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
