#ifndef COSTMAP_2D_STATIC_LAYER_H_
#define COSTMAP_2D_STATIC_LAYER_H_

#include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <map_msgs/OccupancyGridUpdate.h>
// #include <message_filters/subscriber.h>
#include <string>
#include <geometry_msgs/TransformStamped.h>

namespace costmap_2d
{

class StaticLayer : public CostmapLayer
{
public:
  StaticLayer();
  virtual ~StaticLayer();
  virtual void onInitialize();
  virtual void activate();
  virtual void deactivate();
  virtual void reset();

  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                            double* max_x, double* max_y);
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);

  virtual void matchSize();
  virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
  virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
protected:
  virtual unsigned char interpretValue(unsigned char value);
  unsigned char* threshold_;
  std::string base_frame_id_;
  std::string global_frame_;  ///< @brief The global frame for the costmap
  std::string map_frame_;  /// @brief frame that map is located in
  std::string map_topic_;  // Hiep thêm vào mục đich phân biết zones
  bool subscribe_to_updates_;
  bool map_received_;
  bool has_updated_data_;
  unsigned int x_, y_, width_, height_;
  bool track_unknown_space_;
  bool use_maximum_;
  bool first_map_only_;      ///< @brief Store the first static map and reuse it on reinitializing
  bool trinary_costmap_;
  bool map_shutdown_ = false;
  bool map_update_shutdown_ = false;
  // ros::Subscriber map_sub_, map_update_sub_;
  unsigned char lethal_threshold_, unknown_cost_value_;
  
private:
  /**
   * @brief  Callback to update the costmap's map from the map_server
   * @param new_map The map to put into the costmap. The origin of the new
   * map along with its size will determine what parts of the costmap's
   * static map are overwritten.
   */
  // void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
  // dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
  bool getParams();
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_STATIC_LAYER_H_
