/*********************************************************************
 * recovery_core — CollisionChecker dựng trên Costmap2DROBOT.
 *
 * Author: DuongTD
 *********************************************************************/
#include <recovery_core/adapters/costmap_collision_checker.h>

#include <algorithm>
#include <cmath>
#include <utility>

#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/footprint.h>

namespace recovery_core
{

void CostmapCollisionChecker::setFootprintOverride(
    std::vector<robot_geometry_msgs::Point> footprint)
{
  footprint_override_ = std::move(footprint);
}

void CostmapCollisionChecker::setSampleStep(double step_m)
{
  if (std::isfinite(step_m) && step_m > 0.0)
  {
    sample_step_ = step_m;
  }
}

double CostmapCollisionChecker::pointCost(const robot_costmap_2d::Costmap2DROBOT& costmap,
                                          double wx, double wy) const
{
  // Lấy lại con trỏ lưới MỖI LẦN: LayeredCostmap có thể thay Costmap2D bên dưới giữa hai cycle.
  const robot_costmap_2d::Costmap2D* grid = costmap.getCostmap();
  if (grid == nullptr)
  {
    return kOutsideMap;
  }

  unsigned int mx = 0;
  unsigned int my = 0;
  if (!grid->worldToMap(wx, wy, mx, my))
  {
    return kOutsideMap;
  }

  const unsigned char cost = grid->getCost(mx, my);

  if (cost == robot_costmap_2d::NO_INFORMATION)
  {
    return kUnknown;
  }

  // INSCRIBED_INFLATED_OBSTACLE nghĩa là tâm robot đặt ở đây thì đã chạm vật cản — chặn như lethal.
  if (cost == robot_costmap_2d::LETHAL_OBSTACLE ||
      cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
  {
    return kLethal;
  }

  return static_cast<double>(cost);
}

double CostmapCollisionChecker::lineCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double x0,
                                         double y0, double x1, double y1) const
{
  const double length = std::hypot(x1 - x0, y1 - y0);
  const int steps = std::max(1, static_cast<int>(std::ceil(length / sample_step_)));

  double worst = 0.0;
  for (int i = 0; i <= steps; ++i)
  {
    const double t = static_cast<double>(i) / static_cast<double>(steps);
    const double cost = pointCost(costmap, x0 + t * (x1 - x0), y0 + t * (y1 - y0));
    if (cost < 0.0)
    {
      return cost;  // Bất kỳ mã lỗi nào cũng chặn ngay, không đi tiếp trên đoạn này.
    }
    worst = std::max(worst, cost);
  }
  return worst;
}

double CostmapCollisionChecker::footprintCost(double x, double y, double theta) const
{
  if (costmap_ == nullptr)
  {
    // Không có costmap thì không khẳng định được chỗ này đi được — trả về "chặn" chứ không phải 0.
    return kOutsideMap;
  }

  const std::vector<robot_geometry_msgs::Point>& spec =
      footprint_override_.empty() ? costmap_->getRobotFootprint() : footprint_override_;

  if (spec.size() < 3)
  {
    // Footprint suy biến -> coi robot là một điểm, giống CostmapModel.
    return pointCost(*costmap_, x, y);
  }

  std::vector<robot_geometry_msgs::Point> oriented;
  robot_costmap_2d::transformFootprint(x, y, theta, spec, oriented);

  double worst = 0.0;
  for (std::size_t i = 0; i < oriented.size(); ++i)
  {
    const std::size_t j = (i + 1) % oriented.size();
    const double cost =
        lineCost(*costmap_, oriented[i].x, oriented[i].y, oriented[j].x, oriented[j].y);
    if (cost < 0.0)
    {
      return cost;
    }
    worst = std::max(worst, cost);
  }

  return worst;
}

}  // namespace recovery_core