#ifndef OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_
#define OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_

#include <atomic>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "rclcpp/rclcpp.hpp"
#include "obstacle_scanner/msg/obstacle_array.hpp"
#include "tf2_ros/buffer.h"

namespace obstacle_nav2
{

class ObstacleArrayLayer : public nav2_costmap_2d::CostmapLayer
{
public:
  struct CircleObstacle
  {
    double center_x{0.0};
    double center_y{0.0};
    double radius{0.0};
  };

  struct SnapshotBounds
  {
    bool valid{false};
    double min_x{0.0};
    double min_y{0.0};
    double max_x{0.0};
    double max_y{0.0};
  };

  ObstacleArrayLayer();
  ~ObstacleArrayLayer() override;

  void onInitialize() override;
  void updateBounds(
    double robot_x, double robot_y, double robot_yaw,
    double * min_x, double * min_y,
    double * max_x, double * max_y) override;
  void updateCosts(
    nav2_costmap_2d::Costmap2D & master_grid,
    int min_i, int min_j, int max_i, int max_j) override;
  void reset() override;
  bool isClearable() override;
  void activate() override;
  void deactivate() override;

  static void rasterizeCircle(
    nav2_costmap_2d::Costmap2D & grid,
    double cx, double cy, double radius,
    double resolution, double origin_x, double origin_y);
  static double clampRadius(double radius, double min_r, double max_r, double default_r);
  static std::vector<CircleObstacle> transformSnapshot(
    const obstacle_scanner::msg::ObstacleArray & snapshot,
    const std::string & global_frame,
    tf2_ros::Buffer & tf_buffer,
    const rclcpp::Duration & timeout);
  static SnapshotBounds applySnapshot(
    nav2_costmap_2d::Costmap2D & grid,
    const std::vector<CircleObstacle> & obstacles,
    double resolution, double origin_x, double origin_y);

private:
  void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg);
  static SnapshotBounds mergeBounds(
    const SnapshotBounds & first, const SnapshotBounds & second);
  void clearExpiredObstacles(const rclcpp::Time & now);

  // Note: node_ (LifecycleNode::WeakPtr), tf_, layered_costmap_, and name_
  // are inherited from nav2_costmap_2d::Layer.

  rclcpp::Subscription<obstacle_scanner::msg::ObstacleArray>::SharedPtr obstacle_sub_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> callback_executor_;
  std::thread callback_thread_;
  std::atomic_bool callback_stop_{false};
  std::mutex data_mutex_;

  rclcpp::Time last_obstacle_time_;
  bool has_received_obstacles_{false};
  SnapshotBounds current_bounds_;
  SnapshotBounds pending_clear_bounds_;
  std::uint64_t bounds_generation_{0};
  std::uint64_t bounds_generation_used_{0};

  std::string topic_;
  std::string global_frame_;
  double obstacle_timeout_{0.5};
  double transform_tolerance_{0.2};
  double default_obstacle_radius_{0.05};
  double minimum_obstacle_radius_{0.02};
  double maximum_obstacle_radius_{0.50};
  double extra_inflation_{0.02};
};

}  // namespace obstacle_nav2

#endif  // OBSTACLE_NAV2__OBSTACLE_ARRAY_LAYER_HPP_
