#pragma once

#include <memory>
#include <mutex>
#include <string>
#include <vector>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

namespace saltybot_depth_costmap
{

struct ObstaclePoint
{
  double wx;  // world x (m)
  double wy;  // world y (m)
};

/**
 * DepthCostmapLayer — Nav2 costmap2d plugin (Issue #532)
 *
 * Converts RealSense D435i depth images to a Nav2 costmap layer.
 *
 * Pipeline:
 *   1. Subscribe to /camera/depth/image_rect_raw (16UC1, mm) + camera_info
 *   2. Back-project depth pixels to 3D using camera intrinsics
 *   3. Transform points to costmap global_frame via TF2
 *   4. Apply height filter (min_height .. max_height above ground)
 *   5. Mark obstacle cells as LETHAL_OBSTACLE
 *   6. Inflate nearby cells as INSCRIBED_INFLATED_OBSTACLE (in-layer inflation)
 *
 * Nav2 yaml usage:
 *   local_costmap:
 *     plugins: ["obstacle_layer", "voxel_layer", "depth_costmap_layer", "inflation_layer"]
 *     depth_costmap_layer:
 *       plugin: "saltybot_depth_costmap::DepthCostmapLayer"
 *       enabled: true
 *       depth_topic: /camera/depth/image_rect_raw
 *       camera_info_topic: /camera/depth/camera_info
 *       min_height: 0.05          # m — floor clearance
 *       max_height: 0.80          # m — ceiling cutoff
 *       obstacle_range: 3.5       # m — max marking range
 *       clearing_range: 4.0       # m — max clearing range
 *       inflation_radius: 0.10    # m — in-layer inflation radius
 *       downsample_factor: 4      # process 1 of N rows/cols
 */
class DepthCostmapLayer : public nav2_costmap_2d::Layer
{
public:
  DepthCostmapLayer();
  ~DepthCostmapLayer() override = default;

  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 { return true; }

private:
  void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg);
  void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr msg);
  void processDepthImage(const sensor_msgs::msg::Image::SharedPtr & img);

  // ROS interfaces
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_sub_;
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr info_sub_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  // Camera intrinsics (from camera_info)
  double fx_{0.0}, fy_{0.0}, cx_{0.0}, cy_{0.0};
  bool have_camera_info_{false};
  std::string camera_frame_;

  // Plugin parameters
  std::string depth_topic_;
  std::string camera_info_topic_;
  double min_height_{0.05};        // m — filter floor
  double max_height_{0.80};        // m — filter ceiling
  double obstacle_range_{3.5};     // m — max marking range (from camera)
  double clearing_range_{4.0};     // m — max clearing range
  double inflation_radius_{0.10};  // m — in-layer inflation
  int    downsample_factor_{4};    // skip N-1 of every N pixels

  // Latest processed obstacle points in world frame
  std::vector<ObstaclePoint> obstacle_points_;
  std::mutex points_mutex_;
  bool need_update_{false};

  // Dirty bounds for updateBounds()
  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;

  // For throttled logging outside onInitialize
  rclcpp::Logger logger_;
  rclcpp::Clock::SharedPtr clock_;
};

}  // namespace saltybot_depth_costmap
