#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

#include <robot/node_handle.h>
#include <score_algorithm/score_algorithm.h>

#include <boost/dll/alias.hpp>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
#include <tf3/exceptions.h>
#include <tf3/utils.h>
#include <robot_nav_msgs/Path.h>
#include <kalman/kalman.h>
#include <vector>

#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/conversions.h>

namespace mkt_algorithm
{
  namespace diff
  {
    /**
     * @class PredictiveTrajectory
     * @brief Standard PredictiveTrajectory-like ScoreAlgorithm
     */
    class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
    {
    public:
      PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};

      virtual ~PredictiveTrajectory();

      // Standard ScoreAlgorithm Interface
      /**
       * @brief Initialize parameters as needed
       * @param nh NodeHandle to read parameters from
       */
      virtual void initialize(
          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;

      /**
       * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
       *
       * Subclasses may overwrite. Return false in case there is any error.
       *
       * @param pose Current pose (costmap frame)
       * @param velocity Current velocity
       * @param goal The final goal (costmap frame)
       * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
       */
      virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
                           const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
                           double &x_direction, double &y_direction, double &theta_direction) override;

      /**
       * @brief Calculating algorithm
       * @param pose
       * @param velocity
       * @param traj
       */
      virtual mkt_msgs::Trajectory2D calculator(
          const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

      /**
       * @brief Reset all data
       */
      virtual void reset() override;

      /**
       * @brief Stoping move navigation
       */
      virtual void stop() override;

      /**
       * @brief resume move navigation after stopped
       */
      virtual void resume() override;

      /**
       * @brief Create a new PredictiveTrajectory instance
       * @return A pointer to the new PredictiveTrajectory instance
       */
      static score_algorithm::ScoreAlgorithm::Ptr create();

    protected:
      inline double sign(double x)
      {
        return x < 0.0 ? -1.0 : 1.0;
      }

      /**
       * @brief Initialize parameters
       */
      virtual void getParams();

      /**
       * @brief Initialize Kalman filter
       */
      virtual void initKalmanFilter();

      /**
       * @brief Dynamically adjust look ahead distance based on the speed
       * @param velocity
       * @return look ahead distance
       */
      double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity);

      /**
       * @brief Get lookahead point on the global plan
       * @param lookahead_dist
       * @param global_plan
       * @return lookahead point
       */
      std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
      getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);

      /**
       * @brief Create carrot message
       * @param carrot_pose
       * @return carrot message
       */
      robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);

      /**
       * @brief Prune global plan
       * @param tf
       * @param pose
       * @param global_plan
       * @param dist_behind_robot
       * @return true if plan is pruned, false otherwise
       */
      bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose,
                           robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);

      /**
       * @brief  Transforms the global plan of the robot from the planner frame to the local frame (modified).
       *
       * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
       * such that the index of the current goal pose is returned as well as
       * the transformation between the global plan and the planning frame.
       * @param tf A reference to a tf buffer
       * @param global_plan The plan to be transformed
       * @param pose The pose of the robot
       * @param costmap A reference to the costmap being used so the window size for transforming can be computed
       * @param global_frame The frame to transform the plan to
       * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
       * @param[out] transformed_plan Populated with the transformed plan
       * @return \c true if the global plan is transformed, \c false otherwise
       */
      bool transformGlobalPlan(
          TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
          const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
          robot_nav_2d_msgs::Path2D &transformed_plan);

      /**
       * @brief Should rotate to path
       * @param carrot_pose
       * @param angle_to_path
       * @return true if should rotate, false otherwise
       */
      bool shouldRotateToPath(
          const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);

      /**
       * @brief Rotate to heading
       * @param  angle_to_path
       * @param  velocity The velocity of the robot
       * @param  cmd_vel The velocity commands to be filled
       */
      void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);

      /**
       * @brief  the robot is moving acceleration limits
       * @param  velocity The velocity of the robot
       * @param  cmd_vel The velocity commands
       * @param  cmd_vel_result The velocity commands result
       */
      void moveWithAccLimits(
          const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result);

      /**
       * @brief Stop the robot taking into account acceleration limits
       * @param  pose The pose of the robot in the global frame
       * @param  velocity The velocity of the robot
       * @param  cmd_vel The velocity commands to be filled
       * @return  True if a valid trajectory was found, false otherwise
       */
      bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);

      /**
       * @brief Apply constraints
       * @param dist_error
       * @param lookahead_dist
       * @param curvature
       * @param curr_speed
       * @param pose_cost
       * @param linear_vel
       * @param sign
       */
      void applyConstraints(
          const double &dist_error, const double &lookahead_dist,
          const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
          const double &pose_cost, double &linear_vel, double &sign_x);

      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);

      /**
       * @brief Cost at pose
       * @param x
       * @param y
       * @return cost
       */
      double costAtPose(const double &x, const double &y);

      void updateCostAtOffset(
          TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose,
          double x_offset, double y_offset, double &cost_left, double &cost_right);

      double computeDecelerationFactor(double remaining_distance, double decel_distance);

      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);

      bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle, 
                                    double amplitude_threshold = 0.3, size_t window_size = 20);

      void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose);

      std::vector<double> angle_history_;
      size_t window_size_;

      bool initialized_;
      bool nav_stop_, avoid_obstacles_;
      robot::NodeHandle nh_, nh_priv_;
      std::string frame_id_path_;
      std::string robot_base_frame_;

      robot_nav_2d_msgs::Pose2DStamped goal_;
      robot_nav_2d_msgs::Path2D global_plan_;
      robot_nav_2d_msgs::Path2D compute_plan_;
      robot_nav_2d_msgs::Path2D transform_plan_;
      robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;

      double x_direction_, y_direction_, theta_direction_;
      double max_robot_pose_search_dist_;
      double global_plan_prune_distance_{1.0};

      // Lookahead
      bool use_velocity_scaled_lookahead_dist_;
      double lookahead_time_;
      double lookahead_dist_;
      double min_lookahead_dist_;
      double max_lookahead_dist_;

      // journey
      double min_journey_squared_{1e9};
      double max_journey_squared_{1e9};

      // Rotate to heading
      bool use_rotate_to_heading_;
      double rotate_to_heading_min_angle_;

      double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
      double min_path_distance_, max_path_distance_;

      // Regulated linear velocity scaling
      bool use_regulated_linear_velocity_scaling_;
      double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
      double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;

      double rot_stopped_velocity_, trans_stopped_velocity_;

      double min_approach_linear_velocity_;
      double regulated_linear_scaling_min_radius_;
      double regulated_linear_scaling_min_speed_;

      bool use_cost_regulated_linear_velocity_scaling_;
      double inflation_cost_scaling_factor_;
      double cost_scaling_dist_, cost_scaling_gain_;

      double cost_left_goal_, cost_right_goal_;
      double cost_left_side_ , cost_right_side_;
      double center_cost_;

      // Control frequency
      double control_duration_;
      std::vector<robot_geometry_msgs::Point> footprint_spec_;

      robot::Time last_actuator_update_;
      boost::shared_ptr<KalmanFilter> kf_;
      int m_, n_;
      Eigen::MatrixXd A;
      Eigen::MatrixXd C;
      Eigen::MatrixXd Q;
      Eigen::MatrixXd R;
      Eigen::MatrixXd P;

    }; // class PredictiveTrajectory

  } // namespace diff

} // namespace mkt_algorithm

#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__