/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Author: Kevin Lee
 *********************************************************************/
#ifndef NAV_MOVE_BASE_ACTION_H_
#define NAV_MOVE_BASE_ACTION_H_

#include <vector>
#include <string>
#include <tf3/buffer_core.h>
#include <yaml-cpp/yaml.h>


// interfaces headers
#include <move_base_core/navigation.h>
#include <nav_core/base_global_planner.h>
#include <nav_core/base_local_planner.h>
#include <nav_core/recovery_behavior.h>

// boost headers
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/recursive_mutex.hpp>

#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_nav_msgs/GetPlan.h>
#include <robot_protocol_msgs/Order.h>

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

namespace move_base
{

  enum MoveBaseState
  {
    PLANNING,
    CONTROLLING,
    CLEARING
  };

  enum RecoveryTrigger
  {
    PLANNING_R,
    CONTROLLING_R,
    OSCILLATION_R
  };

  struct MoveBaseConfig 
  {
    std::string base_global_planner;
  };

  /**
   * @class MoveBase
   * @brief A class that uses the actionlib::ActionServer interface that moves the robot base to a goal location.
   */
  class MoveBase : public robot::move_base_core::BaseNavigation
  {
  public:
    /**
     * @brief  Constructor for the actions
     * @param name The name of the action
     * @param tf A reference to a TransformListener
     */
    MoveBase(robot::TFListenerPtr tf);
    MoveBase();

    /**
     * @brief Initialize the navigation system.
     * @param tf Shared pointer to the TF listener or buffer.
     */
    virtual void initialize(robot::TFListenerPtr tf) override;

    /**
     * @brief Set the robot's footprint (outline shape) in the global frame.
     *        This can be used for planning or collision checking.
     *
     * @param fprt A vector of points representing the robot's footprint polygon.
     *             The points should be ordered counter-clockwise.
     */
    virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) override;

    /**
     * @brief Send a goal for the robot to navigate to.
     * @param goal              Target pose in the global frame.
     * @param xy_goal_tolerance Acceptable error in X/Y (meters).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if goal was accepted and sent successfully.
     */
    virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a goal for the robot to navigate to.
     * @param msg               Order message.
     * @param goal              Target pose in the global frame.
     * @param xy_goal_tolerance Acceptable error in X/Y (meters).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if goal was accepted and sent successfully.
     */
    virtual bool moveTo(const robot_protocol_msgs::Order &msg,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a docking goal to a predefined marker (e.g. charger).
     * @param maker             Marker name or ID.
     * @param goal              Target pose for docking.
     * @param xy_goal_tolerance Acceptable XY error (meters).
     * @param yaw_goal_tolerance Acceptable heading error (radians).
     * @return True if docking command succeeded.
     */
    virtual bool dockTo(const std::string &maker,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Send a docking goal to a predefined marker (e.g. charger).
     * @param msg               Order message.
     * @param goal              Target pose for docking.
     * @param xy_goal_tolerance Acceptable XY error (meters).
     * @param yaw_goal_tolerance Acceptable heading error (radians).
     * @return True if docking command succeeded.
     */
    virtual bool dockTo(const robot_protocol_msgs::Order &msg,
                        const robot_geometry_msgs::PoseStamped &goal,
                        double xy_goal_tolerance = 0.0,
                        double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Move straight toward the target position (X-axis).
     * @param msg               Order message.
     * @param goal              Target pose.
     * @param xy_goal_tolerance Acceptable positional error (meters).
     * @return True if command issued successfully.
     */
    virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
                                double xy_goal_tolerance = 0.0) override;

    /**
     * @brief Rotate in place to align with target orientation.
     * @param goal              Pose containing desired heading (only Z-axis used).
     * @param yaw_goal_tolerance Acceptable angular error (radians).
     * @return True if rotation command was sent successfully.
     */
    virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
                          double yaw_goal_tolerance = 0.0) override;

    /**
     * @brief Pause the robot's movement (e.g. during obstacle avoidance).
     */
    virtual void pause() override;

    /**
     * @brief Resume motion after a pause.
     */
    virtual void resume() override;

    /**
     * @brief Cancel the current goal and stop the robot.
     */
    virtual void cancel() override;

    /**
     * @brief Send direct linear velocity command.
     * @param linear Linear velocity vector (x, y, z).
     * @return True if the command was accepted.
     */
    virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) override;

    /**
     * @brief Send limited angular velocity command.
     * @param angular Angular velocity vector (x, y, z).
     * @return True if the command was accepted.
     */
    virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) override;

    /**
     * @brief Get the robot’s pose as a PoseStamped.
     * @param pose Output parameter with the robot’s current pose.
     * @return True if pose was successfully retrieved.
     */
    virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) override;

    /**
     * @brief Get the robot’s pose as a 2D pose.
     * @param pose Output parameter with the robot’s current 2D pose.
     * @return True if pose was successfully retrieved.
     */
    virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) override;


    /**
     * @brief Get the robot’s twist.
     * @param twist Output parameter with the robot’s current twist.
     * @return True if twist was successfully retrieved.
     */
    virtual bool getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) override;

    /**
     * @brief Get the navigation feedback.
     * @param feedback Output parameter with the navigation feedback.
     * @return True if feedback was successfully retrieved.
     */
    virtual bool getFeedback(robot::move_base_core::NavFeedback &feedback) override;

    /**
     * @brief  Destructor - Cleans up
     */
    virtual ~MoveBase();

    /**
     * @brief Create a new MoveBase
     * @return A shared pointer to the new MoveBase
     */
    static robot::move_base_core::BaseNavigation::Ptr create();

  private:

    /**
     * @brief  Performs a control cycle
     * @param goal A reference to the goal to pursue
     * @return True if processing of the goal is done, false otherwise
     */
     bool executeCycle(robot_geometry_msgs::PoseStamped &goal);

    /**
     * @brief Set velocity for yaw goal tolerance of the robot
     * @param yaw_goal_tolerance the value command
     */
    void setYawGoalTolerance(double yaw_goal_tolerance);

    /**
     * @brief Set velocity for yaw goal tolerance of the robot
     * @param xy_goal_tolerance the value command
     */
    void setXyGoalTolerance(double xy_goal_tolerance);

    /**
     * @brief Swaps the current global planner with a new one specified by name
     * @param base_global_planner The name of the new global planner to use
     */
    void swapPlanner(std::string base_global_planner);

    /**
     * @brief  Make a new global plan
     * @param  goal The goal to plan to
     * @param  plan Will be filled in with the plan made by the planner
     * @return  True if planning succeeds, false otherwise
     */
    bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);

    /**
     * @brief  Load the recovery behaviors for the navigation stack from the parameter server
     * @return True if the recovery behaviors were loaded successfully, false otherwise
     */
    bool loadRecoveryBehaviors(const robot::NodeHandle &node);

    /**
     * @brief  Loads the default recovery behaviors for the navigation stack
     */
    void loadDefaultRecoveryBehaviors();

    /**
     * @brief  Clears obstacles within a window around the robot
     * @param size_x The x size of the window
     * @param size_y The y size of the window
     */
    void clearCostmapWindows(double size_x, double size_y);

    /**
     * @brief  Publishes a velocity command of zero to the base
     */
    void publishZeroVelocity();

    /**
     * @brief  Reset the state of the move_base action and send a zero velocity command to the base
     */
    void resetState();

    // void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);

    void planThread();

    // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);

    bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);

    bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);

    double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);

    robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);

    // /**
    //  * @brief This is used to wake the planner at periodic intervals.
    //  */
    // void wakePlanner(const robot::TimerEvent &event);

  private:
    bool initialized_;
    robot::TFListenerPtr tf_;
    robot::NodeHandle private_nh_;

    boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
    boost::function<nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
    boost::function<nav_core::RecoveryBehavior::Ptr()> recovery_loader_;

    nav_core::BaseLocalPlanner::Ptr tc_;
    nav_core::BaseGlobalPlanner::Ptr planner_;
    std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
    
    robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
    robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
    std::string robot_base_frame_, global_frame_;
    std::vector<std::string> recovery_behavior_names_;
    unsigned int recovery_index_;
    bool recovery_behavior_enabled_;

    robot_geometry_msgs::PoseStamped global_pose_;
    double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
    double planner_patience_, controller_patience_;
    int32_t max_planning_retries_;
    uint32_t planning_retries_;
    double conservative_reset_dist_, clearing_radius_;
    // robot::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
    // robot::Subscriber goal_sub_;
    // robot::ServiceServer make_plan_srv_, clear_costmaps_srv_;
    bool shutdown_costmaps_, clearing_rotation_allowed_;
    bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
    double oscillation_timeout_, oscillation_distance_;

    MoveBaseState state_;
    RecoveryTrigger recovery_trigger_;

    robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
    robot_geometry_msgs::PoseStamped oscillation_pose_;

    // set up plan triple buffer
    std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
    std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
    std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;

    // set up the planner's thread
    bool runPlanner_;
    boost::recursive_mutex planner_mutex_;
    boost::condition_variable_any planner_cond_;
    robot_geometry_msgs::PoseStamped planner_goal_;
    boost::thread *planner_thread_;

    move_base::MoveBaseConfig last_config_;
    move_base::MoveBaseConfig default_config_;

    bool setup_, p_freq_change_, c_freq_change_;
    bool new_global_plan_;
    bool cancel_ctr_;
    bool pause_ctr_, paused_;
    double min_approach_linear_velocity_{0.1};
    robot_geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
    robot_geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
    double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;

    // defined planner name
    std::string position_planner_name_{"MKTLocalPlanner"};
    std::string docking_planner_name_{"NAVDockingLocalPlanner"};
    std::string go_straight_planner_name_{"NAVGoStraightLocalPlanner"};
    std::string rotate_planner_name_{"NAVRotateLocalPlanner"};
  };

} // namespace move_base

#endif // NAV_MOVE_BASE_ACTION_H_