#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
#define COSTMAP_2D_LAYERED_COSTMAP_H_

#include <costmap_2d/cost_values.h>   // Chứa các hằng số giá trị chi phí (ví dụ: FREE_SPACE, LETHAL_OBSTACLE, NO_INFORMATION)
#include <costmap_2d/layer.h>         // Lớp cơ sở cho các lớp (layer) con: static layer, obstacle layer, inflation layer...
#include <costmap_2d/costmap_2d.h>    // Định nghĩa lớp Costmap2D (bản đồ 2D cơ bản)
#include <vector>
#include <string>

namespace costmap_2d
{

class Layer;  // Khai báo trước để dùng trong vector plugin

/**
 * @class LayeredCostmap
 * @brief Lớp này quản lý nhiều "layer" (tầng bản đồ) khác nhau và kết hợp chúng thành một bản đồ chi phí tổng hợp.
 *        Mỗi layer có thể chứa thông tin khác nhau: chướng ngại vật, vùng lạm phát, vùng cấm, v.v.
 */
class LayeredCostmap
{
public:
  /**
   * @brief Hàm khởi tạo LayeredCostmap.
   * @param global_frame: Tên frame toàn cục (ví dụ: "map" hoặc "odom").
   * @param rolling_window: Nếu true, bản đồ sẽ "trượt" theo robot (rolling window).
   * @param track_unknown: Nếu true, sẽ lưu trữ vùng chưa biết (NO_INFORMATION).
   */
  LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown);

  /**
   * @brief Hàm huỷ lớp LayeredCostmap.
   */
  ~LayeredCostmap();

  /**
   * @brief Cập nhật bản đồ tổng hợp từ tất cả các layer.
   * @param robot_x, robot_y, robot_yaw: vị trí và hướng hiện tại của robot.
   * @note Hàm này gọi updateBounds() và updateCosts() trên từng layer.
   */
  void updateMap(double robot_x, double robot_y, double robot_yaw);

  /// @brief Trả về frame toàn cục (ví dụ "map").
  inline const std::string& getGlobalFrameID() const noexcept
  {
    return global_frame_;
  }

  /**
   * @brief Thay đổi kích thước bản đồ (số ô, độ phân giải, gốc).
   * @param size_x, size_y: kích thước theo ô (cells).
   * @param resolution: độ phân giải (mỗi ô = bao nhiêu mét).
   * @param origin_x, origin_y: toạ độ gốc của bản đồ.
   * @param size_locked: nếu true, không cho phép thay đổi kích thước sau này.
   */
  void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y,
                 bool size_locked = false);

  /**
   * @brief Lấy vùng bản đồ bị thay đổi gần nhất sau lần cập nhật gần nhất.
   * @param [out] minx, miny, maxx, maxy: toạ độ của vùng được cập nhật.
   */
  void getUpdatedBounds(double& minx, double& miny, double& maxx, double& maxy)
  {
    minx = minx_;
    miny = miny_;
    maxx = maxx_;
    maxy = maxy_;
  }

  /**
   * @brief Kiểm tra xem tất cả các layer hiện tại có dữ liệu mới nhất (current) hay không.
   * @return true nếu tất cả layer đều cập nhật.
   */
  bool isCurrent();

  /**
   * @brief Trả về con trỏ đến bản đồ Costmap2D cơ sở (master map).
   */
  Costmap2D* getCostmap()
  {
    return &costmap_;
  }

  /**
   * @brief Kiểm tra xem bản đồ có phải kiểu "rolling window" không.
   */
  bool isRolling()
  {
    return rolling_window_;
  }

  /**
   * @brief Kiểm tra xem bản đồ có đang lưu trữ vùng "chưa biết" không (NO_INFORMATION).
   */
  bool isTrackingUnknown()
  {
    return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
  }

  /**
   * @brief Trả về danh sách các plugin (layer) đã được nạp.
   * @return vector các shared_ptr<Layer>.
   */
  std::vector<boost::shared_ptr<Layer> >* getPlugins()
  {
    return &plugins_;
  }

  /**
   * @brief Thêm một plugin (layer) mới vào bản đồ.
   * @param plugin: con trỏ thông minh đến lớp layer.
   */
  void addPlugin(boost::shared_ptr<Layer> plugin)
  {
    plugins_.push_back(plugin);
  }

  /**
   * @brief Kiểm tra xem bản đồ có bị khoá kích thước không.
   */
  bool isSizeLocked()
  {
    return size_locked_;
  }

  /**
   * @brief Lấy giới hạn vùng bản đồ được cập nhật trong lần gọi update gần nhất.
   * @param [out] x0, xn, y0, yn: chỉ số ô (cells) bắt đầu và kết thúc.
   */
  void getBounds(unsigned int* x0, unsigned int* xn, unsigned int* y0, unsigned int* yn)
  {
    *x0 = bx0_;
    *xn = bxn_;
    *y0 = by0_;
    *yn = byn_;
  }

  /**
   * @brief Kiểm tra xem lớp LayeredCostmap đã được khởi tạo hoàn chỉnh chưa.
   */
  bool isInitialized()
  {
      return initialized_;
  }

  /**
   * @brief Cập nhật footprint (hình dạng chiếm chỗ của robot).
   *        Đồng thời tính lại bán kính bao quanh và nội tiếp.
   *        Gọi hàm onFootprintChanged() của tất cả layer.
   * @param footprint_spec: vector các điểm (geometry_msgs::geometry_msgs::Point) mô tả đa giác footprint.
   */
  void setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec);

  /**
   * @brief Trả về footprint hiện tại của robot.
   */
  const std::vector<geometry_msgs::Point>& getFootprint() { return footprint_; }

  /**
   * @brief Bán kính đường tròn bao ngoài (circumscribed radius):
   *        bán kính nhỏ nhất của đường tròn chứa toàn bộ robot.
   */
  double getCircumscribedRadius() { return circumscribed_radius_; }

  /**
   * @brief Bán kính đường tròn nội tiếp (inscribed radius):
   *        bán kính lớn nhất của đường tròn nằm hoàn toàn bên trong footprint robot.
   */
  double getInscribedRadius() { return inscribed_radius_; }

private:
  Costmap2D costmap_;   ///< Bản đồ 2D chính (lưu ma trận chi phí tổng hợp)
  std::string global_frame_; ///< Frame toàn cục (thường là "map" hoặc "odom")

  bool rolling_window_; ///< Nếu true, bản đồ sẽ di chuyển theo robot (bản đồ trượt)

  bool current_; ///< Cờ đánh dấu tất cả các layer có dữ liệu mới nhất hay không
  double minx_, miny_, maxx_, maxy_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ thế giới)
  unsigned int bx0_, bxn_, by0_, byn_; ///< Giới hạn vùng bản đồ được cập nhật (tọa độ lưới)

  std::vector<boost::shared_ptr<Layer> > plugins_; ///< Danh sách các layer plugin (ví dụ: StaticLayer, ObstacleLayer, InflationLayer)

  bool initialized_; ///< Đã được khởi tạo hoàn toàn hay chưa
  bool size_locked_; ///< Có cho phép thay đổi kích thước bản đồ sau này không

  double circumscribed_radius_, inscribed_radius_; ///< Hai bán kính của footprint robot (bao ngoài và nội tiếp)
  std::vector<geometry_msgs::Point> footprint_; ///< Đa giác mô tả footprint robot
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_LAYERED_COSTMAP_H_
