#include "toid_costmaps/rival_layer.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

namespace toid
{

void RivalLayer::onInitialize()
{
  auto node = node_.lock();
  if (!node) {
    return;
  }
}

void RivalLayer::activate()
{
  auto node = node_.lock();

  nav2_util::declare_parameter_if_not_declared(
    node, name_ + ".rival_size", rclcpp::ParameterValue(0.15));
  node->get_parameter(name_ + ".rival_size", rival_size_);

  using namespace std::placeholders;
  rival_sub_ = node->create_subscription<Rivals>(
    "/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1));
}

void RivalLayer::deactivate()
{
  auto node = node_.lock();
  rival_sub_.reset();
  rivals_.reset();
}

void RivalLayer::rival_cb(Rivals::SharedPtr msg)
{
  if (msg->point.size() != 0 || debounce_++ > 10) {
    rivals_ = msg;
    debounce_ = 0;
  }
}

void RivalLayer::updateBounds(
  double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
{
  touch(1.50, 1.0, min_x, min_y, max_x, max_y);
  touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
}

void RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
{
  geometry_msgs::msg::TransformStamped tf_msg;
  if(!rivals_) {
    return;
  }

  try {
    tf_msg = tf_->lookupTransform(
      layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp);
  } catch (const tf2::TransformException & e) {
    RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap");
    return;
  }

  for (const auto & rival : rivals_->point) {
    geometry_msgs::msg::Point point;
    tf2::doTransform(rival, point, tf_msg);
    placeRival(grid, point.x, point.y);
  }
}

void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y)
{
  unsigned int mx, my;
  if (!grid.worldToMap(x, y, mx, my)) {
    return;  // Center is outside the map bounds
  }

  double res = grid.getResolution();
  int cell_radius = static_cast<int>(rival_size_ / res);

  int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
  int max_i =
    std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
  int min_j = std::max(0, static_cast<int>(my) - cell_radius);
  int max_j =
    std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(my) + cell_radius);

  for (int i = min_i; i <= max_i; ++i) {
    for (int j = min_j; j <= max_j; ++j) {
      double di = static_cast<double>(i) - static_cast<double>(mx);
      double dj = static_cast<double>(j) - static_cast<double>(my);
      double distance_sq = di * di + dj * dj;
      if (distance_sq <= static_cast<double>(cell_radius * cell_radius)) {
        grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
      }
    }
  }
}

}  // namespace toid

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer);