#ifndef __AMR_PUBLISHER_H_INCLUDED_
#define __AMR_PUBLISHER_H_INCLUDED_

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

#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Twist.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_utils/conversions.h>
#include <nav_2d_utils/conversions.h>

#include <nav_2d_msgs/Path2D.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <move_base_core/navigation.h>
#include <memory>
#include <string>

namespace amr_control
{
  struct PlannerObject
  {
    // ROS publishers
    ros::Publisher pub_;
    ros::Publisher update_pub_;
    ros::Publisher footprint_pub_;
    ros::Publisher plan_pub_;
    ros::Timer timer_;
    std::string topic_;
    bool active_;
  };

  /**
   * @class AmrPublisher
   * @brief Publisher class for publishing costmap data from move_base_core to RViz
   * 
   * This class converts robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
   * and publishes global and local costmaps to ROS topics that RViz can visualize.
   */
  class AmrPublisher
  {
  public:
    /**
     * @brief Constructor
     * @param nh ROS NodeHandle for creating publishers
     * @param move_base_ptr Shared pointer to BaseNavigation instance
     */
    AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
    
    /**
     * @brief Destructor
     */
    virtual ~AmrPublisher();

    /**
     * @brief Publish global plan to RViz
     */
    void publishGlobalPlan();

    /**
     * @brief Publish local plan to RViz
     */
    void publishLocalPlan();

    /**
     * @brief Publish global costmap to RViz
     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
     */
    void publishGlobalCostmap(double publish_rate = 0.0);

    /**
     * @brief Publish local costmap to RViz
     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
     */
    void publishLocalCostmap(double publish_rate = 0.0);

    /**
     * @brief Publish both global and local costmaps
     * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
     */
    void publishCostmaps(double publish_rate = 0.0);

    /**
     * @brief Publish global footprint to RViz
     */
    void publishGlobalFootprint();

    /**
     * @brief Publish local footprint to RViz
     */
    void publishLocalFootprint();

    /**
     * @brief Publish both global and local footprints
     */
    void publishFootprints();

    /**
     * @brief Start publishing costmaps at a fixed rate
     * @param global_rate Publishing rate for global costmap (Hz). If 0, don't publish global.
     * @param local_rate Publishing rate for local costmap (Hz). If 0, don't publish local.
     */
    void startPublishing(double global_rate = 1.0, double local_rate = 5.0);

    /**
     * @brief Stop publishing costmaps
     */
    void stopPublishing();

    /**
     * @brief Check if publishing is active
     * @return True if publishing is active (either global or local)
     */
    bool isPublishing() const { return global_planner_obj_.active_ || local_planner_obj_.active_ || cmd_vel_publishing_active_; }

    /**
     * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
     * @param rate Publishing rate in Hz (default: 20.0 Hz)
     */
     void startCmdVelPublishing(double rate = 20.0);

     /**
      * @brief Stop publishing cmd_vel
      */
     void stopCmdVelPublishing();

    /**
     * @brief Start publishing plans at a fixed rate
     * @param global_rate Publishing rate for global plan (Hz). If 0, don't publish global.
     * @param local_rate Publishing rate for local plan (Hz). If 0, don't publish local.
     */
    void startPlanPublishing(double global_rate = 10.0, double local_rate = 10.0);

    /**
     * @brief Stop publishing plans
     */
    void stopPlanPublishing();

  private:

    /**
     * @brief Convert robot_nav_2d_msgs::Path2D to nav_msgs::Path
     * @param robot_path Input robot_nav_2d_msgs::Path2D
     * @param nav_path Output nav_msgs::Path
     */
    void convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, 
                          nav_msgs::Path &nav_path);

    /**
     * @brief Callback for new subscription to global costmap
     * @param pub SingleSubscriberPublisher for the new subscription
     */
    void onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub);

    /**
     * @brief Callback for new subscription to local costmap
     * @param pub SingleSubscriberPublisher for the new subscription
     */
    void onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub);

    /**
     * @brief Convert robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
     * @param robot_grid Input robot_nav_msgs::OccupancyGrid
     * @param nav_grid Output nav_msgs::OccupancyGrid
     */
    void convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, 
                                    nav_msgs::OccupancyGrid &nav_grid);

    void convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update, 
                                           map_msgs::OccupancyGridUpdate &grid_update);

    /**
     * @brief Convert robot_geometry_msgs::PolygonStamped to geometry_msgs::PolygonStamped
     * @param robot_footprint Input robot_geometry_msgs::PolygonStamped
     * @param nav_footprint Output geometry_msgs::PolygonStamped
     */
    void convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint, 
                                     geometry_msgs::PolygonStamped &nav_footprint);

    /**
     * @brief Timer callback for publishing global costmap
     * @param event Timer event
     */
    void globalCostmapTimerCallback(const ros::TimerEvent &event);

    /**
     * @brief Timer callback for publishing local costmap
     * @param event Timer event
     */
    void localCostmapTimerCallback(const ros::TimerEvent &event);

    /**
     * @brief Publish cmd_vel command
     */
    void publishCmdVel();

    /**
     * @brief WallTimer callback for publishing cmd_vel
     * @param event WallTimer event
     */
    void cmdVelTimerCallback(const ros::WallTimerEvent &event);

    /**
     * @brief Timer callback for updating global plan
     * @param event Timer event
     */
    void globalPlanTimerCallback(const ros::TimerEvent &event);

    /**
     * @brief Timer callback for updating local plan
     * @param event Timer event
     */
    void localPlanTimerCallback(const ros::TimerEvent &event);

    ros::NodeHandle nh_;
    robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;

    PlannerObject global_planner_obj_;
    PlannerObject local_planner_obj_;
    ros::Publisher cmd_vel_pub_;
    
    // ros::WallTimer for cmd_vel publishing
    ros::WallTimer cmd_vel_timer_;
    bool cmd_vel_publishing_active_;

    // Timers for plan publishing
    ros::Timer global_plan_timer_;
    ros::Timer local_plan_timer_;
    bool global_plan_publishing_active_;
    bool local_plan_publishing_active_;

    // Cached plans with timestamps
    robot_nav_2d_msgs::Path2D cached_global_plan_;
    robot_nav_2d_msgs::Path2D cached_local_plan_;
  };

} // namespace amr_control

#endif // __AMR_PUBLISHER_H_INCLUDED_

