#pragma once

#include <mutex>
#include <string>

#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "rclcpp/rclcpp.hpp"

namespace saltybot_segmentation_costmap
{

/**
 * SegmentationCostmapLayer
 *
 * Nav2 costmap2d plugin that subscribes to /segmentation/costmap
 * (nav_msgs/OccupancyGrid published by sidewalk_seg_node) and merges
 * traversability costs into the Nav2 local_costmap.
 *
 * Merging strategy (combination_method parameter):
 *   "max"      — keep the higher cost between existing and new (default, conservative)
 *   "override" — always write the new cost, replacing existing
 *   "min"      — keep the lower cost (permissive)
 *
 * Cost mapping from OccupancyGrid int8 to Nav2 costmap uint8:
 *   -1   (unknown) → not written (cell unchanged)
 *   0    (free)    → FREE_SPACE (0)
 *   1–99 (cost)    → scaled to Nav2 range [1, INSCRIBED_INFLATED_OBSTACLE-1]
 *   100  (lethal)  → LETHAL_OBSTACLE (254)
 *
 * Add to Nav2 local_costmap params:
 *   local_costmap:
 *     plugins: ["voxel_layer", "inflation_layer", "segmentation_layer"]
 *     segmentation_layer:
 *       plugin: "saltybot_segmentation_costmap::SegmentationCostmapLayer"
 *       enabled: true
 *       topic: /segmentation/costmap
 *       combination_method: max
 *       max_obstacle_distance: 0.0   (use all cells)
 */
class SegmentationCostmapLayer : public nav2_costmap_2d::Layer
{
public:
  SegmentationCostmapLayer();
  ~SegmentationCostmapLayer() 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 segCostmapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);

  /**
   * Map OccupancyGrid int8 value to Nav2 costmap uint8 cost.
   *   -1  → leave unchanged (return false)
   *   0   → FREE_SPACE (0)
   *   1–99 → 1 to INSCRIBED_INFLATED_OBSTACLE-1 (linear scale)
   *   100 → LETHAL_OBSTACLE (254)
   */
  static bool occupancyToCost(int8_t occ, unsigned char & cost_out);

  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_;
  nav_msgs::msg::OccupancyGrid::SharedPtr latest_grid_;
  std::mutex grid_mutex_;

  std::string topic_;
  std::string combination_method_;  // "max", "override", "min"
  bool        enabled_;

  // Track dirty bounds for updateBounds()
  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
  bool   need_update_;
};

}  // namespace saltybot_segmentation_costmap
