/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the institute nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christoph Rösmann
 *********************************************************************/

#ifndef TIMED_ELASTIC_BAND_H_
#define TIMED_ELASTIC_BAND_H_

#include <ros/ros.h>
#include <ros/assert.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/tf.h>

#include <complex>
#include <iterator>

#include <teb_local_planner/obstacles.h>

// G2O Types
#include <teb_local_planner/g2o_types/vertex_pose.h>
#include <teb_local_planner/g2o_types/vertex_timediff.h>


namespace teb_local_planner
{

//! Container of poses that represent the spatial part of the trajectory
typedef std::vector<VertexPose*> PoseSequence;
//! Container of time differences that define the temporal of the trajectory
typedef std::vector<VertexTimeDiff*> TimeDiffSequence;


/**
 * @class TimedElasticBand
 * @brief Class that defines a trajectory modeled as an elastic band with augmented tempoarl information.
 * 
 * All trajectory related methods (initialization, modifying, ...) are implemented inside this class. \n
 * Let \f$ Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \f$ be a sequence of poses, \n
 * in which \f$ \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \f$ denotes a single pose of the robot. \n
 * The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between
 * two consecutive poses, resuting in a sequence of \c n-1 time intervals \f$ \Delta T_i \f$: \n
 * \f$ \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \f$. \n
 * Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration.
 * The tuple of both sequences defines the underlying trajectory.
 * 
 * Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. \n
 * TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory.
 * 
 * @todo Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class.
 */
class TimedElasticBand
{
public:

  /**
   * @brief Construct the class
   */
  TimedElasticBand();
  
  /**
   * @brief Destruct the class
   */
  virtual ~TimedElasticBand();

  
  
  /** @name Access pose and timediff sequences */
  //@{
  
  /**
   * @brief Access the complete pose sequence 
   * @return reference to the pose sequence
   */
  PoseSequence& poses() {return pose_vec_;};
  
   /**
   * @brief Access the complete pose sequence (read-only)
   * @return const reference to the pose sequence
   */
  const PoseSequence& poses() const {return pose_vec_;};
  
  /**
   * @brief Access the complete timediff sequence 
   * @return reference to the dimediff sequence
   */
  TimeDiffSequence& timediffs() {return timediff_vec_;};
  
  /**
   * @brief Access the complete timediff sequence 
   * @return reference to the dimediff sequence
   */
  const TimeDiffSequence& timediffs() const {return timediff_vec_;};  
  
  /**
   * @brief Access the time difference at pos \c index of the time sequence
   * @param index element position inside the internal TimeDiffSequence
   * @return reference to the time difference at pos \c index
   */
  double& TimeDiff(int index)
  {
    ROS_ASSERT(index<sizeTimeDiffs()); 
    return timediff_vec_.at(index)->dt();
  }
  
  /**
   * @brief Access the time difference at pos \c index of the time sequence (read-only)
   * @param index element position inside the internal TimeDiffSequence
   * @return const reference to the time difference at pos \c index
   */
  const double& TimeDiff(int index) const
  {
    ROS_ASSERT(index<sizeTimeDiffs()); 
    return timediff_vec_.at(index)->dt();
  }
  
  /**
   * @brief Access the pose at pos \c index of the pose sequence
   * @param index element position inside the internal PoseSequence
   * @return reference to the pose at pos \c index
   */
  PoseSE2& Pose(int index) 
  {
    ROS_ASSERT(index<sizePoses());
    return pose_vec_.at(index)->pose();
  }
  
  /**
   * @brief Access the pose at pos \c index of the pose sequence (read-only)
   * @param index element position inside the internal PoseSequence
   * @return const reference to the pose at pos \c index
   */
  const PoseSE2& Pose(int index) const 
  {
    ROS_ASSERT(index<sizePoses());
    return pose_vec_.at(index)->pose();
  }
  
  /**
   * @brief Access the last PoseSE2 in the pose sequence
   */
  PoseSE2& BackPose() {return pose_vec_.back()->pose(); }
  
  /**
   * @brief Access the last PoseSE2 in the pose sequence (read-only) 
   */
  const PoseSE2& BackPose() const {return pose_vec_.back()->pose();}
  
  /**
   * @brief Access the last TimeDiff in the time diff sequence
   */ 
  double& BackTimeDiff() {return timediff_vec_.back()->dt(); }
  
  /**
   * @brief Access the last TimeDiff in the time diff sequence (read-only)
   */  
  const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); }
  
  /**
   * @brief Access the vertex of a pose at pos \c index for optimization purposes
   * @param index element position inside the internal PoseSequence
   * @return Weak raw pointer to the pose vertex at pos \c index
   */ 
  VertexPose* PoseVertex(int index) 
  {
    ROS_ASSERT(index<sizePoses());
    return pose_vec_.at(index);
  }
  
  /**
   * @brief Access the vertex of a time difference at pos \c index for optimization purposes
   * @param index element position inside the internal TimeDiffSequence
   * @return Weak raw pointer to the timediff vertex at pos \c index
   */  
  VertexTimeDiff* TimeDiffVertex(int index) 
  {
    ROS_ASSERT(index<sizeTimeDiffs()); 
    return timediff_vec_.at(index);
  }
  
  //@}
  
  
  
  /** @name Append new elements to the pose and timediff sequences */
  //@{
  
  /**
   * @brief Append a new pose vertex to the back of the pose sequence 
   * @param pose PoseSE2 to push back on the internal PoseSequence
   * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
   */
  void addPose(const PoseSE2& pose, bool fixed=false);  

  /**
   * @brief Append a new pose vertex to the back of the pose sequence 
   * @param position 2D vector representing the position part
   * @param theta yaw angle representing the orientation part
   * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
   */
  void addPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed=false);
  
  /**
   * @brief Append a new pose vertex to the back of the pose sequence 
   * @param x x-coordinate of the position part
   * @param y y-coordinate of the position part
   * @param theta yaw angle representing the orientation part
   * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
   */
  void addPose(double x, double y, double theta, bool fixed=false);
  
  /**
   * @brief Append a new time difference vertex to the back of the time diff sequence 
   * @param dt time difference value to push back on the internal TimeDiffSequence
   * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner)
   */
  void addTimeDiff(double dt, bool fixed=false);
  
  /**
   * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
   * @param pose PoseSE2 to push back on the internal PoseSequence
   * @param dt time difference value to push back on the internal TimeDiffSequence
   * @warning 	Since the timediff is defined to connect two consecutive poses, this call is only 
   * 		allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...):
   * 		therefore add a single pose using addPose() first!
   */
  void addPoseAndTimeDiff(const PoseSE2& pose, double dt);
  
  /**
   * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
   * @param position 2D vector representing the position part
   * @param theta yaw angle representing the orientation part
   * @param dt time difference value to push back on the internal TimeDiffSequence
   * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
   */
  void addPoseAndTimeDiff(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, double dt);
  
  /**
   * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences)
   * @param x x-coordinate of the position part
   * @param y y-coordinate of the position part
   * @param theta yaw angle representing the orientation part
   * @param dt time difference value to push back on the internal TimeDiffSequence
   * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt)
   */
  void addPoseAndTimeDiff(double x, double y, double theta, double dt);
  
  //@}
  

  /** @name Insert new elements and remove elements of the pose and timediff sequences */
  //@{

  /**
   * @brief Insert a new pose vertex at pos. \c index to the pose sequence
   * @param index element position inside the internal PoseSequence
   * @param pose PoseSE2 element to insert into the internal PoseSequence
   */
  void insertPose(int index, const PoseSE2& pose);
  
  /**
   * @brief Insert a new pose vertex at pos. \c index to the pose sequence
   * @param index element position inside the internal PoseSequence
   * @param position 2D vector representing the position part
   * @param theta yaw-angle representing the orientation part
   */
  void insertPose(int index, const Eigen::Ref<const Eigen::Vector2d>& position, double theta);
  
  /**
   * @brief Insert a new pose vertex at pos. \c index to the pose sequence
   * @param index element position inside the internal PoseSequence
   * @param x x-coordinate of the position part
   * @param y y-coordinate of the position part
   * @param theta yaw-angle representing the orientation part
   */
  void insertPose(int index, double x, double y, double theta);
  
  /**
   * @brief Insert a new timediff vertex at pos. \c index to the timediff sequence
   * @param index element position inside the internal TimeDiffSequence
   * @param dt timediff value
   */   
  void insertTimeDiff(int index, double dt);
    
  /**
   * @brief Delete pose at pos. \c index in the pose sequence
   * @param index element position inside the internal PoseSequence
   */
  void deletePose(int index);
  
  /**
   * @brief Delete multiple (\c number) poses starting at pos. \c index in the pose sequence
   * @param index first element position inside the internal PoseSequence
   * @param number number of elements that should be deleted
   */
  void deletePoses(int index, int number);

  /**
   * @brief Delete pose at pos. \c index in the timediff sequence
   * @param index element position inside the internal TimeDiffSequence
   */
  void deleteTimeDiff(int index);
	
  /**
   * @brief Delete multiple (\c number) time differences starting at pos. \c index in the timediff sequence
   * @param index first element position inside the internal TimeDiffSequence
   * @param number number of elements that should be deleted
   */
  void deleteTimeDiffs(int index, int number);
  
  //@}
  
  
  /** @name Init the trajectory */
  //@{
  
  /**
   * @brief Initialize a trajectory between a given start and goal pose.
   * 
   * The implemented algorithm subsamples the straight line between
   * start and goal using a given discretiziation width. \n
   * The discretization width can be defined in the euclidean space
   * using the \c diststep parameter. Each time difference between two consecutive
   * poses is initialized to \c timestep. \n
   * If the \c diststep is chosen to be zero, 
   * the resulting trajectory contains the start and goal pose only.
   * @param start PoseSE2 defining the start of the trajectory
   * @param goal PoseSE2 defining the goal of the trajectory (final pose)
   * @param diststep euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples)
   * @param max_vel_x maximum translational velocity used for determining time differences
   * @param min_samples Minimum number of samples that should be initialized at least
   * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
   * @return true if everything was fine, false otherwise
   */
  bool initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double max_vel_x=0.5, int min_samples = 3, bool guess_backwards_motion = false);
  
  
  /**
   * @brief Initialize a trajectory from a generic 2D reference path.
   * 
   * The temporal information is determined using a given maximum velocity 
   * (2D vector containing the translational and angular velocity). \n
   * A constant velocity profile is implemented. \n
   * A possible maximum acceleration is considered if \c max_acceleration param is provided.
   * 
   * Since the orientation is not included in the reference path, it can be provided seperately
   * (e.g. from the robot pose and robot goal). \n
   * Otherwise the goal heading will be used as start and goal orientation. \n
   * The orientation along the trajectory will be determined using the connection vector
   * between two consecutive positions of the reference path.
   * 
   * The reference path is provided using a start and end iterator to a container class.
   * You must provide a unary function that accepts the dereferenced iterator and returns
   * a copy or (const) reference to an Eigen::Vector2d type containing the 2d position.
   * 
   * @param path_start start iterator of a generic 2d path
   * @param path_end end iterator of a generic 2d path
   * @param fun_position unary function that returns the Eigen::Vector2d object
   * @param max_vel_x maximum translational velocity used for determining time differences
   * @param max_vel_theta maximum angular velocity used for determining time differences
   * @param max_acc_x specify to satisfy a maxmimum transl. acceleration and decceleration (optional)
   * @param max_acc_theta specify to satisfy a maxmimum angular acceleration and decceleration (optional)
   * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
   * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
   * @param min_samples Minimum number of samples that should be initialized at least
   * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot
   * @tparam BidirIter Bidirectional iterator type
   * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
   * @return true if everything was fine, false otherwise
   * @remarks Use \c boost::none to skip optional arguments.
   */ 
  template<typename BidirIter, typename Fun>
  bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
		      boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
		      boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false);  
  
  /**
   * @brief Initialize a trajectory from a reference pose sequence (positions and orientations).
   *
   * This method initializes the timed elastic band using a pose container
   * (e.g. as local plan from the ros navigation stack). \n
   * The initial time difference between two consecutive poses can be uniformly set
   * via the argument \c dt.
   * @param plan vector of geometry_msgs::PoseStamped
   * @param max_vel_x maximum translational velocity used for determining time differences
   * @param max_vel_theta maximum rotational velocity used for determining time differences
   * @param estimate_orient if \c true, calculate orientation using the straight line distance vector between consecutive poses
   *                        (only copy start and goal orientation; recommended if no orientation data is available).
   * @param min_samples Minimum number of samples that should be initialized at least
   * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if \c estimate_orient is enabled.
   * @return true if everything was fine, false otherwise
   */
  bool initTrajectoryToGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double max_vel_x, double max_vel_theta, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false);


  ROS_DEPRECATED bool initTEBtoGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double timestep=1, int min_samples = 3, bool guess_backwards_motion = false)
  {
    ROS_WARN_ONCE("initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: timestep has been replaced by max_vel_x. \
                   this deprecated method sets max_vel_x = 1. Please update your code.");
    return initTrajectoryToGoal(start, goal, diststep, timestep, min_samples, guess_backwards_motion);
  }

  template<typename BidirIter, typename Fun>
  ROS_DEPRECATED bool initTEBtoGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
          boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
          boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples = 3, bool guess_backwards_motion = false)
  {
    return initTrajectoryToGoal<BidirIter, Fun>(path_start, path_end, fun_position, max_vel_x, max_vel_theta,
                                                max_acc_x, max_acc_theta, start_orientation, goal_orientation, min_samples, guess_backwards_motion);
  }

  ROS_DEPRECATED bool initTEBtoGoal(const std::vector<geometry_msgs::PoseStamped>& plan, double dt, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false)
  {
    ROS_WARN_ONCE("initTEBtoGoal is deprecated and has been replaced by initTrajectoryToGoal. The signature has changed: dt has been replaced by max_vel_x. \
                   this deprecated method sets max_vel = 1. Please update your code.");
    return initTrajectoryToGoal(plan, 1.0, 1.0, estimate_orient, min_samples, guess_backwards_motion);
  }

  
  //@}
  
  /** @name Update and modify the trajectory */
  //@{
  
  
  /**
   * @brief Hot-Start from an existing trajectory with updated start and goal poses.
   *
   * This method updates a previously optimized trajectory with a new start and/or a new goal pose. \n
   * The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start. \n
   * Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed.
   * The optimizer has to smooth the trajectory in TebOptimalPlanner. \n
   * 
   * @todo Smooth the trajectory here and test the performance improvement of the optimization.
   * @todo Implement a updateAndPruneTEB based on a new reference path / pose sequence.
   * 
   * @param new_start New start pose (optional)
   * @param new_goal New goal pose (optional)
   * @param min_samples Specify the minimum number of samples that should at least remain in the trajectory
   */  
  void updateAndPruneTEB(boost::optional<const PoseSE2&> new_start, boost::optional<const PoseSE2&> new_goal, int min_samples = 3);
  
  
  /**
   * @brief Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution.
   *
   * Resizing the trajectory is helpful e.g. for the following scenarios:
   * 
   * 	- Obstacles requires the teb to be extended in order to 
   * 	  satisfy the given discritization width (plan resolution)
   *      and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$
   * 	  After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version.
   *    - If the distance to the goal state is getting smaller,
   *      dt is decreasing as well. This leads to a heavily
   *      fine-grained discretization in combination with many
   *      discrete poses. Thus, the computation time will
   *      be/remain high and in addition numerical instabilities
   *      can appear (e.g. due to the division by a small \f$ \Delta T_i \f$).
   *
   * The implemented strategy checks all timediffs \f$ \Delta T_i \f$ and
   * 
   * 	- inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$
   *    - removes a sample if \f$ \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \f$
   * 
   * Each call only one new sample (pose-dt-pair) is inserted or removed.
   * @param dt_ref reference temporal resolution
   * @param dt_hysteresis hysteresis to avoid oscillations
   * @param min_samples minimum number of samples that should be remain in the trajectory after resizing
   * @param max_samples maximum number of samples that should not be exceeded during resizing
   * @param fast_mode if true, the trajectory is iterated once to insert or erase points; if false the trajectory
   *                  is repeatedly iterated until no poses are added or removed anymore
   */    
  void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false);

  /**
   * @brief Set a pose vertex at pos \c index of the pose sequence to be fixed or unfixed during optimization.
   * @param index index to the pose vertex
   * @param status if \c true, the vertex will be fixed, otherwise unfixed
   */
  void setPoseVertexFixed(int index, bool status);
  
  /**
   * @brief Set a timediff vertex at pos \c index of the timediff sequence to be fixed or unfixed during optimization.
   * @param index index to the timediff vertex
   * @param status if \c true, the vertex will be fixed, otherwise unfixed
   */
  void setTimeDiffVertexFixed(int index, bool status);
  
  /**
   * @brief clear all poses and timediffs from the trajectory.
   * The pose and timediff sequences will be empty and isInit() will return \c false
   */
  void clearTimedElasticBand();
  
  //@}
  
  
  /** @name Utility and status methods */
  //@{
  
  /**
   * @brief Find the closest point on the trajectory w.r.t. to a provided reference point.
   * 
   * This function can be useful to find the part of a trajectory that is close to an obstacle.
   * 
   * @todo implement a more efficient version that first performs a coarse search.
   * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
   *       Allows simple comparisons starting from the middle of the trajectory.
   * 
   * @param ref_point reference point (2D position vector)
   * @param[out] distance [optional] the resulting minimum distance
   * @param begin_idx start search at this pose index
   * @return Index to the closest pose in the pose sequence
   */
  int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_point, double* distance = NULL, int begin_idx=0) const;

  /**
   * @brief Find the closest point on the trajectory w.r.t. to a provided reference line.
   * 
   * This function can be useful to find the part of a trajectory that is close to an (line) obstacle.
   * 
   * @todo implement a more efficient version that first performs a coarse search.
   * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
   *       Allows simple comparisons starting from the middle of the trajectory.
   * 
   * @param ref_line_start start of the reference line (2D position vector)
	 * @param ref_line_end end of the reference line (2D position vector)
   * @param[out] distance [optional] the resulting minimum distance
   * @return Index to the closest pose in the pose sequence
   */
  int findClosestTrajectoryPose(const Eigen::Ref<const Eigen::Vector2d>& ref_line_start, const Eigen::Ref<const Eigen::Vector2d>& ref_line_end, double* distance = NULL) const;

  /**
   * @brief Find the closest point on the trajectory w.r.t. to a provided reference polygon.
   * 
   * This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle.
   * 
   * @todo implement a more efficient version that first performs a coarse search.
   * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance:
   *       Allows simple comparisons starting from the middle of the trajectory.
   * 
   * @param vertices vertex container containing Eigen::Vector2d points (the last and first point are connected)
   * @param[out] distance [optional] the resulting minimum distance
   * @return Index to the closest pose in the pose sequence
   */
  int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const;

  /**
   * @brief Find the closest point on the trajectory w.r.t to a provided obstacle type
   * 
   * This function can be useful to find the part of a trajectory that is close to an obstacle.
   * The method is calculates appropriate distance metrics for point, line and polygon obstacles.
   * For all unknown obstacles the centroid is used.
   *
   * @param obstacle Subclass of the Obstacle base class
   * @param[out] distance [optional] the resulting minimum distance
   * @return Index to the closest pose in the pose sequence
   */
  int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const;
  
  
  /**
   * @brief Get the length of the internal pose sequence
   */
  int sizePoses() const {return (int)pose_vec_.size();};
  
  /**
   * @brief Get the length of the internal timediff sequence
   */
  int sizeTimeDiffs() const {return (int)timediff_vec_.size();};
  
  /**
   * @brief Check whether the trajectory is initialized (nonzero pose and timediff sequences)
   */
  bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();}

  /**
   * @brief Calculate the total transition time (sum over all time intervals of the timediff sequence)
   */
  double getSumOfAllTimeDiffs() const;
  
  /**
   * @brief Calculate the estimated transition time up to the pose denoted by index
   * @param index Index of the pose up to which the transition times are summed up
   * @return Estimated transition time up to pose index
   */
  double getSumOfTimeDiffsUpToIdx(int index) const;

  /**
   * @brief Calculate the length (accumulated euclidean distance) of the trajectory
   */
  double getAccumulatedDistance() const;
  
  /**
   * @brief Check if all trajectory points are contained in a specific region
   * 
   * The specific region is a circle around the current robot position (Pose(0)) with given radius \c radius.
   * This method investigates a different radius for points behind the robot if \c max_dist_behind_robot >= 0.
   * @param radius radius of the region with the robot position (Pose(0)) as center
   * @param max_dist_behind_robot A separate radius for trajectory points behind the robot, activated if 0 or positive
   * @param skip_poses If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested.
   * @return \c true, if all tested trajectory points are inside the specified region, \c false otherwise.
   */
  bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0);
  
  
  
  //@}
	
protected:
  PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices
  TimeDiffSequence timediff_vec_;  //!< Internal container storing the sequence of optimzable timediff vertices
  
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace teb_local_planner


// include template implementations / definitions
#include <teb_local_planner/timed_elastic_band.hpp>


#endif /* TIMED_ELASTIC_BAND_H_ */
