/* +------------------------------------------------------------------------+
   |                     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/config/CConfigFileBase.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/round.h>
#include <mrpt/kinematics/CVehicleVelCmd.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TTwist2D.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/nav/holonomic/ClearanceDiagram.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/serialization/CSerializable.h>

#include <cstdint>

namespace mrpt
{
namespace opengl
{
class CSetOfLines;
}
}  // namespace mrpt

namespace mrpt
{
namespace nav
{
/** Defines behaviors for where there is an obstacle *inside* the robot shape
 *right at the beginning of a PTG trajectory.
 *\ingroup nav_tpspace
 * \sa Used in CParameterizedTrajectoryGenerator::COLLISION_BEHAVIOR
 */
enum PTG_collision_behavior_t
{
  /** Favor getting back from too-close (almost collision) obstacles. */
  COLL_BEH_BACK_AWAY = 0,
  /** Totally dissallow any movement if there is any too-close (almost
   collision) obstacles. */
  COLL_BEH_STOP
};

/** \defgroup nav_tpspace TP-Space and PTG classes
 * \ingroup mrpt_nav_grp
 */

/** This is the base class for any user-defined PTG.
 *  There is a class factory interface in
 *CParameterizedTrajectoryGenerator::CreatePTG.
 *
 * Papers:
 *  - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Extending
 *Obstacle Avoidance Methods through Multiple Parameter-Space Transformations",
 *Autonomous Robots, vol. 24, no. 1, 2008.
 *http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf
 *
 * Changes history:
 *	- 30/JUN/2004: Creation (JLBC)
 *	- 16/SEP/2004: Totally redesigned.
 *	- 15/SEP/2005: Totally rewritten again, for integration into MRPT
 *Applications Repository.
 *	- 19/JUL/2009: Simplified to use only STL data types, and created the class
 *factory interface.
 *	- MAY/2016: Refactored into CParameterizedTrajectoryGenerator,
 *CPTG_DiffDrive_CollisionGridBased, PTG classes renamed.
 *	- 2016-2018: Many features added to support "PTG continuation", dynamic
 *paths depending on vehicle speeds, etc.
 *
 *  \ingroup nav_tpspace
 */
class CParameterizedTrajectoryGenerator :
    public mrpt::serialization::CSerializable,
    public mrpt::config::CLoadableOptions
{
  DEFINE_VIRTUAL_SERIALIZABLE(CParameterizedTrajectoryGenerator, mrpt::nav)
 public:
  /** Default ctor. Must call `loadFromConfigFile()` before initialization */
  CParameterizedTrajectoryGenerator() = default;

  /**  Destructor  */
  ~CParameterizedTrajectoryGenerator() override = default;

  /** The class factory for creating a PTG from a list of parameters in a
   *section of a given config file (physical file or in memory).
   *  Possible parameters are:
   *	  - Those explained in
   *CParameterizedTrajectoryGenerator::loadFromConfigFile()
   *	  - Those explained in the specific PTG being created (see list of
   *derived classes)
   *
   * `ptgClassName` can be any PTG class name which has been registered as
   *any other
   * mrpt::serialization::CSerializable class.
   *
   * \exception std::logic_error On invalid or missing parameters.
   */
  static CParameterizedTrajectoryGenerator::Ptr CreatePTG(
      const std::string& ptgClassName,
      const mrpt::config::CConfigFileBase& cfg,
      const std::string& sSection,
      const std::string& sKeyPrefix);

  /** @name Virtual interface of each PTG implementation
   *  @{ */
  /** Gets a short textual description of the PTG and its parameters */
  virtual std::string getDescription() const = 0;

 protected:
  /** Must be called after setting all PTG parameters and before requesting
   * converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
  virtual void internal_initialize(
      const std::string& cacheFilename = std::string(), const bool verbose = true) = 0;
  /** This must be called to de-initialize the PTG if some parameter is to be
   * changed. After changing it, call initialize again */
  virtual void internal_deinitialize() = 0;

 public:
  /** Computes the closest (alpha,d) TP coordinates of the trajectory point
   * closest to the Workspace (WS)
   *   Cartesian coordinates (x,y), relative to the current robot frame.
   * \param[in] x X coordinate of the query point, relative to the robot
   * frame.
   * \param[in] y Y coordinate of the query point, relative to the robot
   * frame.
   * \param[out] out_k Trajectory parameter index (discretized alpha value,
   * 0-based index).
   * \param[out] out_d Trajectory distance, normalized such that D_max
   * becomes 1.
   *
   * \return true if the distance between (x,y) and the actual trajectory
   * point is below the given tolerance (in meters).
   */
  virtual bool inverseMap_WS2TP(
      double x,
      double y,
      int& out_k,
      double& out_normalized_d,
      double tolerance_dist = 0.10) const = 0;

  /** Returns the same than inverseMap_WS2TP() but without any additional
   * cost. The default implementation
   * just calls inverseMap_WS2TP() and discards (k,d). */
  virtual bool PTG_IsIntoDomain(double x, double y) const
  {
    int k;
    double d;
    return inverseMap_WS2TP(x, y, k, d);
  }

  /** Returns true if a given TP-Space point maps to a unique point in
   * Workspace, and viceversa. Default implementation returns `true`. */
  virtual bool isBijectiveAt([[maybe_unused]] uint16_t k, [[maybe_unused]] uint32_t step) const
  {
    return true;
  }
  /** Converts a discretized "alpha" value into a feasible motion command or
   * action. See derived classes for the meaning of these actions */
  virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const = 0;

  /** Returns an empty kinematic velocity command object of the type supported
   * by this PTG.
   * Can be queried to determine the expected kinematic interface of the PTG.
   */
  virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const = 0;

  /** Dynamic state that may affect the PTG path parameterization. \ingroup
   * nav_reactive  */
  struct TNavDynamicState
  {
    /** Current vehicle velocity (local frame of reference) */
    mrpt::math::TTwist2D curVelLocal{0, 0, 0};
    /** Current relative target location */
    mrpt::math::TPose2D relTarget{20.0, .0, .0};
    /** Desired relative speed [0,1] at target. Default=0 */
    double targetRelSpeed{0};

    /** Derived-class specific internal state variables */
    mrpt::containers::yaml internalState;

    TNavDynamicState() = default;
    bool operator==(const TNavDynamicState& o) const;
    inline bool operator!=(const TNavDynamicState& o) const { return !(*this == o); }
    void writeToStream(mrpt::serialization::CArchive& out) const;
    void readFromStream(mrpt::serialization::CArchive& in);
  };

 protected:
  /** Invoked when `m_nav_dyn_state` has changed; gives the PTG the
   * opportunity to react and parameterize paths depending on the
   * instantaneous conditions */
  virtual void onNewNavDynamicState() = 0;

 public:
  virtual void setRefDistance(const double refDist) { refDistance = refDist; }
  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps"
   * along the trajectory.
   * May be actual steps from a numerical integration or an arbitrary small
   * length for analytical PTGs.
   * \sa getAlphaValuesCount() */
  virtual size_t getPathStepCount(uint16_t k) const = 0;

  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at
   * discrete step `step`.
   * \sa getPathStepCount(), getAlphaValuesCount(), getPathTwist() */
  virtual mrpt::math::TPose2D getPathPose(uint16_t k, uint32_t step) const = 0;

  /** Gets velocity ("twist") for path `k` ([0,N-1]=>[-pi,pi] in alpha),
   * at vehicle discrete step `step`. The default implementation in this base
   * class uses numerical differentiation to estimate the twist (vx,vy,omega).
   * Velocity is given in "global" coordinates, relative to the starting pose
   * of the robot at t=0 for this PTG path.
   * \sa getPathStepCount(), getAlphaValuesCount(), getPathPose() */
  virtual mrpt::math::TTwist2D getPathTwist(uint16_t k, uint32_t step) const;

  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): traversed distance at
   * discrete step `step`.
   * \return Distance in pseudometers (real distance, NOT normalized to [0,1]
   * for [0,refDist])
   * \sa getPathStepCount(), getAlphaValuesCount() */
  virtual double getPathDist(uint16_t k, uint32_t step) const = 0;

  /** Returns the duration (in seconds) of each "step"
   * \sa getPathStepCount() */
  virtual double getPathStepDuration() const = 0;

  /** Returns the maximum linear velocity expected from this PTG [m/s] */
  virtual double getMaxLinVel() const = 0;
  /** Returns the maximum angular velocity expected from this PTG [rad/s] */
  virtual double getMaxAngVel() const = 0;

  /** Access path `k` ([0,N-1]=>[-pi,pi] in alpha): largest step count for
   * which the traversed distance is < `dist`
   * \param[in] dist Distance in pseudometers (real distance, NOT normalized
   * to [0,1] for [0,refDist])
   * \return false if no step fulfills the condition for the given trajectory
   * `k` (e.g. out of reference distance).
   * Note that, anyway, the maximum distance (closest point) is returned in
   * `out_step`.
   * \sa getPathStepCount(), getAlphaValuesCount() */
  virtual bool getPathStepForDist(uint16_t k, double dist, uint32_t& out_step) const = 0;

  /** Updates the radial map of closest TP-Obstacles given a single obstacle
   * point at (ox,oy)
   * \param [in,out] tp_obstacles A vector of length `getAlphaValuesCount()`,
   * initialized with `initTPObstacles()` (collision-free ranges, in
   * "pseudometers", un-normalized).
   * \param [in] ox Obstacle point (X), relative coordinates wrt origin of
   * the PTG.
   * \param [in] oy Obstacle point (Y), relative coordinates wrt origin of
   * the PTG.
   * \note The length of tp_obstacles is not checked for efficiency since
   * this method is potentially called thousands of times per
   *  navigation timestap, so it is left to the user responsibility to
   * provide a valid buffer.
   * \note `tp_obstacles` must be initialized with initTPObstacle() before
   * call.
   */
  virtual void updateTPObstacle(double ox, double oy, std::vector<double>& tp_obstacles) const = 0;

  /** Like updateTPObstacle() but for one direction only (`k`) in TP-Space.
   * `tp_obstacle_k` must be initialized with initTPObstacleSingle() before
   * call (collision-free ranges, in "pseudometers", un-normalized). */
  virtual void updateTPObstacleSingle(
      double ox, double oy, uint16_t k, double& tp_obstacle_k) const = 0;

  /** Loads a set of default parameters into the PTG. Users normally will call
   * `loadFromConfigFile()` instead, this method is provided
   * exclusively for the PTG-configurator tool. */
  virtual void loadDefaultParams();

  /** Returns true if it is possible to stop sending velocity commands to the
   * robot and, still, the
   * robot controller will be able to keep following the last sent trajectory
   * ("NOP" velocity commands).
   * Default implementation returns "false". */
  virtual bool supportVelCmdNOP() const;

  /** Returns true if this PTG takes into account the desired velocity at
   * target. \sa updateNavDynamicState() */
  virtual bool supportSpeedAtTarget() const { return false; }
  /** Only for PTGs supporting supportVelCmdNOP(): this is the maximum time
   * (in seconds) for which the path
   * can be followed without re-issuing a new velcmd. Note that this is only
   * an absolute maximum duration,
   * navigation implementations will check for many other conditions. Default
   * method in the base virtual class returns 0.
   * \param path_k Queried path `k` index  [0,N-1] */
  virtual double maxTimeInVelCmdNOP(int path_k) const;

  /** Returns the actual distance (in meters) of the path, discounting
   * possible circular loops of the path (e.g. if it comes back to the
   * origin).
   * Default: refDistance */
  virtual double getActualUnloopedPathLength(  //
      [[maybe_unused]] uint16_t k) const
  {
    return this->refDistance;
  }

  /** Query the PTG for the relative priority factor (0,1) of this PTG, in
   * comparison to others, if the k-th path is to be selected. */
  virtual double evalPathRelativePriority(
      [[maybe_unused]] uint16_t k, [[maybe_unused]] double target_distance) const
  {
    return 1.0;
  }

  /** Returns an approximation of the robot radius. */
  virtual double getMaxRobotRadius() const = 0;
  /** Returns true if the point lies within the robot shape. */
  virtual bool isPointInsideRobotShape(const double x, const double y) const = 0;

  /** Evals the clearance from an obstacle (ox,oy) in coordinates relative to
   * the robot center. Zero or negative means collision. */
  virtual double evalClearanceToRobotShape(const double ox, const double oy) const = 0;

  /** @} */  // --- end of virtual methods

  /** To be invoked by the navigator *before* each navigation step, to let the
   * PTG to react to changing dynamic conditions. * \sa
   * onNewNavDynamicState(), m_nav_dyn_state  */
  void updateNavDynamicState(const TNavDynamicState& newState, const bool force_update = false);
  const TNavDynamicState& getCurrentNavDynamicState() const { return m_nav_dyn_state; }

  /** The path used as default output in, for example, debugDumpInFiles.
   * (Default="./reactivenav.logs/") */
  static std::string& OUTPUT_DEBUG_PATH_PREFIX();

  /** Must be called after setting all PTG parameters and before requesting
   * converting obstacles to TP-Space, inverseMap_WS2TP(), etc. */
  void initialize(const std::string& cacheFilename = std::string(), const bool verbose = true);
  /** This must be called to de-initialize the PTG if some parameter is to be
   * changed. After changing it, call initialize again */
  void deinitialize();
  /** Returns true if `initialize()` has been called and there was no errors,
   * so the PTG is ready to be queried for paths, obstacles, etc. */
  bool isInitialized() const;

  /** Get the number of different, discrete paths in this family */
  uint16_t getAlphaValuesCount() const { return m_alphaValuesCount; }
  /** Get the number of different, discrete paths in this family */
  uint16_t getPathCount() const { return m_alphaValuesCount; }
  /** Alpha value for the discrete corresponding value \sa alpha2index */
  double index2alpha(uint16_t k) const;
  static double Index2alpha(uint16_t k, const unsigned int num_paths);

  /** Discrete index value for the corresponding alpha value \sa index2alpha
   */
  uint16_t alpha2index(double alpha) const;
  static uint16_t Alpha2index(double alpha, const unsigned int num_paths);

  inline double getRefDistance() const { return refDistance; }
  /** Resizes and populates the initial appropriate contents in a vector of
   * tp-obstacles (collision-free ranges, in "pseudometers", un-normalized).
   * \sa updateTPObstacle()  */
  void initTPObstacles(std::vector<double>& TP_Obstacles) const;
  void initTPObstacleSingle(uint16_t k, double& TP_Obstacle_k) const;

  /** When used in path planning, a multiplying factor (default=1.0) for the
   * scores for this PTG. Assign values <1 to PTGs with low priority. */
  double getScorePriority() const { return m_score_priority; }
  void setScorePriorty(double prior) { m_score_priority = prior; }
  unsigned getClearanceStepCount() const { return m_clearance_num_points; }
  void setClearanceStepCount(const unsigned res) { m_clearance_num_points = res; }

  unsigned getClearanceDecimatedPaths() const { return m_clearance_decimated_paths; }
  void setClearanceDecimatedPaths(const unsigned num) { m_clearance_decimated_paths = num; }

  /** Returns the representation of one trajectory of this PTG as a 3D OpenGL
   * object (a simple curved line).
   * \param[in] k The 0-based index of the selected trajectory (discrete
   * "alpha" parameter).
   * \param[out] gl_obj Output object.
   * \param[in] decimate_distance Minimum distance between path points (in
   * meters).
   * \param[in] max_path_distance If >=0, cut the path at this distance (in
   * meters).
   */
  virtual void renderPathAsSimpleLine(
      const uint16_t k,
      mrpt::opengl::CSetOfLines& gl_obj,
      const double decimate_distance = 0.1,
      const double max_path_distance = -1.0) const;

  /** Dump PTG trajectories in four text files:
   * `./reactivenav.logs/PTGs/PTG%i_{x,y,phi,d}.txt`
   * Text files are loadable from MATLAB/Octave, and can be visualized with
   * the script `[MRPT_DIR]/scripts/viewPTG.m`
   * \note The directory "./reactivenav.logs/PTGs" will be created if doesn't
   * exist.
   * \return false on any error writing to disk.
   * \sa OUTPUT_DEBUG_PATH_PREFIX
   */
  bool debugDumpInFiles(const std::string& ptg_name) const;

  /** Parameters accepted by this base class:
   *   - `${sKeyPrefix}num_paths`: The number of different paths in this
   * family (number of discrete `alpha` values).
   *   - `${sKeyPrefix}ref_distance`: The maximum distance in PTGs [meters]
   *   - `${sKeyPrefix}score_priority`: When used in path planning, a
   * multiplying factor (default=1.0) for the scores for this PTG. Assign
   * values <1 to PTGs with low priority.
   */
  void loadFromConfigFile(
      const mrpt::config::CConfigFileBase& cfg, const std::string& sSection) override;
  void saveToConfigFile(
      mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const override;

  /** Auxiliary function for rendering */
  virtual void add_robotShape_to_setOfLines(
      mrpt::opengl::CSetOfLines& gl_shape,
      const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const = 0;

  /** Defines the behavior when there is an obstacle *inside* the robot shape
   * right at the beginning of a PTG trajectory.
   * Default value: COLL_BEH_BACK_AWAY
   */
  static PTG_collision_behavior_t& COLLISION_BEHAVIOR();

  /** Must be called to resize a CD to its correct size, before calling
   * updateClearance() */
  void initClearanceDiagram(ClearanceDiagram& cd) const;

  /** Updates the clearance diagram given one (ox,oy) obstacle point, in
   * coordinates relative
   * to the PTG path origin.
   * \param[in,out] cd The clearance will be updated here.
   * \sa m_clearance_dist_resolution
   */
  void updateClearance(const double ox, const double oy, ClearanceDiagram& cd) const;
  void updateClearancePost(ClearanceDiagram& cd, const std::vector<double>& TP_obstacles) const;

 protected:
  double refDistance{.0};

  /** The number of discrete values for "alpha" between -PI and +PI. */
  uint16_t m_alphaValuesCount{0};
  double m_score_priority{1.0};

  /** Number of steps for the piecewise-constant approximation of clearance
   * from TPS distances [0,1] (Default=5) \sa updateClearance() */
  uint16_t m_clearance_num_points{5};

  /** Number of paths for the decimated paths analysis of clearance */
  uint16_t m_clearance_decimated_paths{15};

  /** Updated before each nav step by  */
  TNavDynamicState m_nav_dyn_state;

  /** Updated in updateNavDynamicState(), contains the path index (k) for the
   * target. */
  uint16_t m_nav_dyn_state_target_k = INVALID_PTG_PATH_INDEX;

  static const uint16_t INVALID_PTG_PATH_INDEX = static_cast<uint16_t>(-1);

  bool m_is_initialized{false};

  /** To be called by implementors of updateTPObstacle() and
   * updateTPObstacleSingle() to
   * honor the user settings regarding COLLISION_BEHAVIOR.
   * \param new_tp_obs_dist The newly determiend collision-free ranges, in
   * "pseudometers", un-normalized, for some "k" direction.
   * \param inout_tp_obs The target where to store the new TP-Obs distance,
   * if it fulfills the criteria determined by the collision behavior.
   */
  void internal_TPObsDistancePostprocess(
      const double ox, const double oy, const double new_tp_obs_dist, double& inout_tp_obs) const;

  virtual void internal_readFromStream(mrpt::serialization::CArchive& in);
  virtual void internal_writeToStream(mrpt::serialization::CArchive& out) const;

 public:
  /** Evals the robot clearance for each robot pose along path `k`, for the
   * real distances in
   * the key of the map<>, then keep in the map value the minimum of its
   * current stored clearance,
   * or the computed clearance. In case of collision, clearance is zero.
   * \param treat_as_obstacle true: normal use for obstacles; false: compute
   * shortest distances to a target point (no collision)
   */
  virtual void evalClearanceSingleObstacle(
      const double ox,
      const double oy,
      const uint16_t k,
      ClearanceDiagram::dist2clearance_t& inout_realdist2clearance,
      bool treat_as_obstacle = true) const;

};  // end of class

/** A list of PTGs (smart pointers) */
using TListPTGPtr = std::vector<mrpt::nav::CParameterizedTrajectoryGenerator::Ptr>;

/** Base class for all PTGs using a 2D polygonal robot shape model.
 *  \ingroup nav_tpspace
 */
class CPTG_RobotShape_Polygonal : public CParameterizedTrajectoryGenerator
{
 public:
  CPTG_RobotShape_Polygonal();
  ~CPTG_RobotShape_Polygonal() override;

  /** @name Robot shape
   * @{ **/
  /** Robot shape must be set before initialization, either from ctor params
   * or via this method. */
  void setRobotShape(const mrpt::math::CPolygon& robotShape);
  const mrpt::math::CPolygon& getRobotShape() const { return m_robotShape; }
  double getMaxRobotRadius() const override;
  double evalClearanceToRobotShape(const double ox, const double oy) const override;
  /** @} */
  bool isPointInsideRobotShape(const double x, const double y) const override;
  void add_robotShape_to_setOfLines(
      mrpt::opengl::CSetOfLines& gl_shape,
      const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const override;

  static void static_add_robotShape_to_setOfLines(
      mrpt::opengl::CSetOfLines& gl_shape,
      const mrpt::poses::CPose2D& origin,
      const mrpt::math::CPolygon& robotShape);

 protected:
  /** Will be called whenever the robot shape is set / updated */
  virtual void internal_processNewRobotShape() = 0;
  mrpt::math::CPolygon m_robotShape;
  double m_robotMaxRadius{.01};
  void loadShapeFromConfigFile(
      const mrpt::config::CConfigFileBase& source, const std::string& section);
  void saveToConfigFile(
      mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const override;
  void internal_shape_loadFromStream(mrpt::serialization::CArchive& in);
  void internal_shape_saveToStream(mrpt::serialization::CArchive& out) const;
  /** Loads a set of default parameters; provided  exclusively for the
   * PTG-configurator tool. */
  void loadDefaultParams() override;
};

/** Base class for all PTGs using a 2D circular robot shape model.
 *  \ingroup nav_tpspace
 */
class CPTG_RobotShape_Circular : public CParameterizedTrajectoryGenerator
{
 public:
  CPTG_RobotShape_Circular();
  ~CPTG_RobotShape_Circular() override;

  /** @name Robot shape
   * @{ **/
  /** Robot shape must be set before initialization, either from ctor params
   * or via this method. */
  void setRobotShapeRadius(const double robot_radius);
  double getRobotShapeRadius() const { return m_robotRadius; }
  double getMaxRobotRadius() const override;
  double evalClearanceToRobotShape(const double ox, const double oy) const override;
  /** @} */
  void add_robotShape_to_setOfLines(
      mrpt::opengl::CSetOfLines& gl_shape,
      const mrpt::poses::CPose2D& origin = mrpt::poses::CPose2D()) const override;

  static void static_add_robotShape_to_setOfLines(
      mrpt::opengl::CSetOfLines& gl_shape,
      const mrpt::poses::CPose2D& origin,
      const double robotRadius);

  bool isPointInsideRobotShape(const double x, const double y) const override;

 protected:
  /** Will be called whenever the robot shape is set / updated */
  virtual void internal_processNewRobotShape() = 0;
  double m_robotRadius{.0};
  void loadShapeFromConfigFile(
      const mrpt::config::CConfigFileBase& source, const std::string& section);
  void saveToConfigFile(
      mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const override;
  void internal_shape_loadFromStream(mrpt::serialization::CArchive& in);
  void internal_shape_saveToStream(mrpt::serialization::CArchive& out) const;
  /** Loads a set of default parameters; provided  exclusively for the
   * PTG-configurator tool. */
  void loadDefaultParams() override;
};
}  // namespace nav
}  // namespace mrpt
