#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.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:
      /**
       * @brief Constructor
       */
      PredictiveTrajectory();

      /**
       * @brief Destructor
       */
      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 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 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 Calculate max kappa
       * @param path
       * @param preview_distance
       * @return max kappa
       */
      double calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance = std::numeric_limits<double>::max());
    
      /**
       * @brief Adjust speed with Hermite trajectory
       * @param velocity
       * @param trajectory
       * @param v_target
       * @param sign_x
       * @return speed
       */
      double adjustSpeedWithHermiteTrajectory(
        const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Path2D &trajectory, double v_target, const double &sign_x
      );

      /**
       * @brief Generate Hermite trajectory
       * @param path
       * @param drive_target
       * @param velocity
       * @param sign_x
       * @param drive_cmd
      * @return trajectory
       */
      robot_nav_2d_msgs::Path2D generateTrajectory(  
        const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target,
        const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd, const double &dt);

      /**
       * @brief Generate trajectory
       * @param path
       * @param sign_x
       * @return trajectory
       */
       robot_nav_2d_msgs::Path2D generateParallelPath(
        const robot_nav_2d_msgs::Path2D &path, const double &sign_x);

      /**
       * @brief Generate Hermite trajectory
       * @param path
       * @param sign_x
       * @return trajectory
       */
      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);

      /**
       * @brief Generate Hermite quadratic trajectory
       * @param pose
       * @param sign_x
       * @return trajectory
       */
      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);

      /**
       * @brief Should rotate to path
       * @param carrot_pose
       * @param velocity
       * @param sign_x
       * @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, 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, const double &dt);

      /**
       * @brief Check if robot should align to final heading (near goal position)
       * @param trajectory The transformed trajectory in robot frame
       * @param goal The goal pose with desired final heading
       * @param velocity Current velocity
       * @param xy_error Output: distance to goal
       * @param heading_error Output: angle difference to goal heading
       * @return true if should enter final heading alignment phase
       */
      bool shouldAlignToFinalHeading(
          const robot_nav_2d_msgs::Path2D &trajectory,
          const robot_nav_2d_msgs::Pose2DStamped &goal,
          const robot_nav_2d_msgs::Twist2D &velocity,
          double &xy_error,
          double &heading_error,
          const double &sign_x);

      /**
       * @brief Arc Motion controller for final heading alignment
       * Smoothly reduces linear velocity while adjusting angular velocity to reach final heading
       * @param xy_error Distance to goal position
       * @param heading_error Angle difference to goal heading
       * @param velocity Current velocity
       * @param sign_x Direction of motion (forward/backward)
       * @param dt Time step
       * @param cmd_vel Output velocity command
       */
      void alignToFinalHeading(
          const double &xy_error,
          const double &heading_error,
          const robot_nav_2d_msgs::Twist2D &velocity,
          const double &sign_x,
          const double &dt,
          robot_nav_2d_msgs::Twist2D &cmd_vel);
      
      /**
       * @brief PID controller
       * @param error
       * @param integral
       * @param last_error
       * @param dt
       * @param kp
       * @param ki
       * @param kd
       * @param output
       */
      void pid(const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output);

      /**
       * @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
       * @param  dt The time step
       */
      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, const double &dt);

      /**
       * @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
       * @param  dt The time step
       * @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, const double &dt);

      /**
       * @brief Compute pure pursuit
       * @param carrot_pose
       * @param drive_target
       * @param velocity
       * @param trajectory
       * @param min_approach_linear_velocity
       * @param sign_x
       * @param dt
       * @param drive_cmd
       */
      void computePurePursuit(
        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
        const robot_nav_2d_msgs::Twist2D &drive_target,
        const robot_nav_2d_msgs::Twist2D &velocity,
        const robot_nav_2d_msgs::Path2D &trajectory,
        const double &min_approach_linear_velocity,
        const double &sign_x,
        const double &dt,
        robot_nav_2d_msgs::Twist2D &drive_cmd);

      /**
       * @brief Apply distance-based speed scaling and acceleration limits
       * @param plan The plan to be scaled
       * @param velocity Current velocity
       * @param drive_cmd Input/output velocity command
       * @param sign_x Direction of motion
       * @param dt Time step
       */
      void applyDistanceSpeedScaling(
        const robot_nav_2d_msgs::Path2D &plan,
        const robot_nav_2d_msgs::Twist2D &velocity,
        robot_nav_2d_msgs::Twist2D &drive_cmd,
        const double &sign_x,
        const double &dt);

      /**
       * @brief Interpolate footprint
       * @param pose
       * @param footprint
       * @param resolution
       * @return interpolated footprint
       */
      std::vector<robot_geometry_msgs::Point> interpolateFootprint(
        const robot_geometry_msgs::Pose2D &pose, const std::vector<robot_geometry_msgs::Point> &footprint, const double &resolution);

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


      bool initialized_;
      bool nav_stop_;
      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_;

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

      // 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 min_path_distance_, max_path_distance_;

      // Final heading alignment (Arc Motion)
      bool use_final_heading_alignment_;
      double final_heading_xy_tolerance_;      // Distance threshold to trigger alignment
      double final_heading_angle_tolerance_;   // Angle threshold to consider heading reached
      double final_heading_min_velocity_;      // Minimum linear velocity during alignment
      double final_heading_kp_angular_;        // Proportional gain for angular control
      double final_heading_ki_angular_;
      double final_heading_kd_angular_;

      double near_goal_heading_integral_;
      double near_goal_heading_last_error_;
      bool near_goal_heading_was_active_;
      
      double min_approach_linear_velocity_;

      bool use_cost_regulated_linear_velocity_scaling_;
      double inflation_cost_scaling_factor_;
      double cost_scaling_dist_, cost_scaling_gain_;

      // 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;

      // Kalman filter tuning (for v and w filtering)
      double kf_q_v_;
      double kf_q_w_;
      double kf_r_v_;
      double kf_r_w_;
      double kf_p0_;
      bool kf_filter_angular_;
#ifdef BUILD_WITH_ROS
      ros::Publisher lookahead_point_pub_;
#endif

    }; // class PredictiveTrajectory

  } // namespace diff

} // namespace mkt_algorithm

#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__