#ifndef _PNKX_LOCAL_PLANNER_H_INCLUDED_
#define _PNKX_LOCAL_PLANNER_H_INCLUDED_

#include <robot/robot.h>
#include <robot_nav_core2/local_planner.h>
#include <score_algorithm/trajectory_generator.h>
#include <score_algorithm/score_algorithm.h>
#include <score_algorithm/goal_checker.h>
#include <pnkx_local_planner/transforms.h>

namespace pnkx_local_planner
{

  /**
   * @class PNKXLocalPlanner
   * @brief Plugin-based flexible local_planner
   */
  class PNKXLocalPlanner : public robot_nav_core2::LocalPlanner
  {
  public:
    /**
     * @brief Constructor that brings up pluginlib loaders
     */
    PNKXLocalPlanner();

    virtual ~PNKXLocalPlanner();

    /**
     * @brief robot_nav_core2 initialization
     * @param name Namespace for this planner
     * @param tf TFListener pointer
     * @param costmap_robot Costmap pointer
     */
    void initialize(robot::NodeHandle &parent, const std::string &name,
                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;

    /**
     * @brief robot_nav_core2 setGoalPose - Sets the global goal pose
     * @param goal_pose The Goal Pose
     */
    void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) override;

    /**
     * @brief robot_nav_core2 setPlan - Sets the global plan
     * @param path The global plan
     */
    void setPlan(const robot_nav_2d_msgs::Path2D &path) override;

    /**
     * @brief robot_nav_core2 getPlan - Gets the current global plan
     * @param path The global plan
     */
    void getPlan(robot_nav_2d_msgs::Path2D &path) override;

    /**
     * @brief robot_nav_core2 getGlobalPlan - Gets the current global plan
     * @param path The global plan
     */
    void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) override;

    /**
     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
     *
     * It is presumed that the global plan is already set.
     *
     * This is mostly a wrapper for the protected computeVelocityCommands
     * function which has additional debugging info.
     *
     * @param pose Current robot pose
     * @param velocity Current robot velocity
     * @return The best command for the robot to drive
     */
    robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
                                                        const robot_nav_2d_msgs::Twist2D &velocity) override;

    /**
     * @brief set velocity for x, y asix of the robot
     * @param linear the velocity command
     * @return True if set is done, false otherwise
     */
    bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;

    /**
     * @brief get velocity for x, y asix of the robot
     * @param linear The velocity command
     * @return True if set is done, false otherwise
     */
    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;

    /**
     * @brief Set velocity theta for z asix of the robot
     * @param angular the command velocity
     * @return True if set is done, false otherwise
     */
    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;

    /**
     * @brief Get velocity theta for z asix of the robot
     * @param direct The velocity command
     * @return True if set is done, false otherwise
     */
    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;

    /**
     * @brief Check if the kinematic parameters are currently locked
     * @return True if the kinematic parameters are locked, false otherwise
     *
     * This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL.
     */
    virtual bool islock() override;

    /**
     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
     *
     * The pose that it checks against is the last pose in the current global plan.
     * The calculation is delegated to the goal_checker plugin.
     *
     * @param pose Current pose
     * @param velocity Current velocity
     * @return True if the robot should be considered as having reached the goal.
     */
    bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;

    /**
     * @brief Create a new PNKXLocalPlanner
     * @return A shared pointer to the new PNKXLocalPlanner
     */
    static robot_nav_core2::LocalPlanner::Ptr create();

  protected:
    /**
     * @brief Lock the kinematic parameters to prevent modifications
     *
     * This function sets the kinematic parameters to a locked state, ensuring that values such as direction (xytheta_direct), velocities, or accelerations cannot be changed until unlocked. Locking during critical operations like in-place rotation may stabilize motion commands but could cause issues if the lock persists too long, preventing necessary updates to odometry data used by AMCL.
     */
    virtual void lock();

    /**
     * @brief Unlock the kinematic parameters to allow modifications
     *
     * This function releases the lock on kinematic parameters, allowing changes to values such as direction, velocities, or accelerations. If the parameters are unlocked prematurely during in-place rotation, sudden changes to motion parameters (e.g., theta direction) could lead to odometry discontinuities, contributing to localization jumps in AMCL.
     */
    virtual void unlock();
    /**
     * @brief robot_nav_core2 initialization other
     */
    virtual void initializeOthers();

    /**
     * @brief robot_nav_core2 reset other
     */
    virtual void reset();

    /**
     * @brief Get parameters
     * @param nh NodeHandle to read parameters from
     */
    virtual void getParams(robot::NodeHandle &nh);

    /**
     * @brief Helper method for preparing for the core scoring algorithm
     *
     * Runs the prepare method on all the critics with freshly transformed data
     */
    virtual void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

    /**
     * @brief Iterate through all the twists and find the best one
     */
    virtual robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity);

    /**
     * @brief Helper method to transform a given pose to the local costmap frame.
     */
    robot_nav_2d_msgs::Pose2DStamped transformPoseToLocal(const robot_nav_2d_msgs::Pose2DStamped &pose);

    // Plugin handling
    std::function<score_algorithm::TrajectoryGenerator::Ptr()> traj_gen_loader_;
    score_algorithm::TrajectoryGenerator::Ptr traj_generator_;

    std::function<score_algorithm::ScoreAlgorithm::Ptr()> nav_algorithm_loader_;
    score_algorithm::ScoreAlgorithm::Ptr nav_algorithm_;
    std::function<score_algorithm::ScoreAlgorithm::Ptr()> rotate_algorithm_loader_;
    score_algorithm::ScoreAlgorithm::Ptr rotate_algorithm_;

    std::function<score_algorithm::GoalChecker::Ptr()> goal_checker_loader_;
    score_algorithm::GoalChecker::Ptr goal_checker_;

    bool initialized_;
    TFListenerPtr tf_;
    std::string name_;
    robot::NodeHandle parent_, planner_nh_;  
    robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
    robot_nav_2d_msgs::Path2D transformed_global_plan_;
    robot_nav_2d_msgs::Path2D local_plan_;
    robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
 
    robot_costmap_2d::Costmap2D *costmap_;
    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
    nav_grid::NavGridInfo info_;
    boost::recursive_mutex configuration_mutex_;
    bool update_costmap_before_planning_;

    bool ret_angle_, ret_nav_;
    double yaw_goal_tolerance_, xy_goal_tolerance_;
    bool lock_;

  }; // PNKXLocalPlanner

} // namespace pnkx_local_planner

#endif // _PNKX_LOCAL_PLANNER_H_INCLUDED_