/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — xoá vật cản đã tích trong costmap. Một tick, không output.
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

#include <algorithm>
#include <cmath>
#include <set>
#include <string>
#include <vector>

#include <boost/dll/alias.hpp>
#include <boost/pointer_cast.hpp>
#include <boost/thread/locks.hpp>
#include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_layer.h>

namespace recovery_plugins
{
namespace
{
constexpr double kDefaultResetDistance = 3.0;  // [m]
constexpr double kMaxResetDistance = 100.0;    // [m] trần vệ sinh cho param sai

std::string leafName(std::string name)
{
  const std::string::size_type slash = name.rfind('/');
  if (slash != std::string::npos)
  {
    name = name.substr(slash + 1);
  }
  return name;
}
}  // namespace

/**
 * @class ClearCostmapRecovery
 * @brief Xoá vùng vật cản đã tích trong các layer được chỉ định.
 *
 * Dùng **hai instance** trong bộ default:
 *  - `conservative_reset` (`invert_area_to_clear: false`) xoá vùng gần robot;
 *  - `aggressive_reset`  (`invert_area_to_clear: true`) xoá mọi thứ **ngoài** vùng đó.
 *
 * Không phát output, hoàn tất trong một tick.
 *
 * @note Cố ý **không** gọi `Costmap2DROBOT::updateMap()`. Hàm đó giữ mutex master rồi chạy toàn bộ
 *       chuỗi layer `updateBounds`/`updateCosts` — cỡ chục tới trăm ms — trong khi behavior này
 *       chạy trên thread phát cmd_vel ở 30 Hz. Costmap tự update ở chu kỳ riêng của nó ngay sau
 *       đó; ép update tại đây chỉ để đổi lấy một chu kỳ control bị lỡ.
 */
class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
{
public:
  ClearCostmapRecovery() = default;

  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
  {
    return std::make_shared<ClearCostmapRecovery>();
  }

  recovery_core::RecoveryOutputType outputKind() const override
  {
    return recovery_core::RecoveryOutputType::kNone;
  }

protected:
  bool onConfigure(robot::NodeHandle& nh) override
  {
    nh.param("reset_distance", reset_distance_, kDefaultResetDistance);
    nh.param("invert_area_to_clear", invert_area_to_clear_, false);
    nh.param("affected_maps", affected_maps_, std::string("both"));

    if (!std::isfinite(reset_distance_) || reset_distance_ <= 0.0 ||
        reset_distance_ > kMaxResetDistance)
    {
      robot::log_warning("[recovery_core] '%s': reset_distance=%.3f m outside (0, %.0f]; using "
                         "%.3f m.", name().c_str(), reset_distance_, kMaxResetDistance,
                         kDefaultResetDistance);
      reset_distance_ = kDefaultResetDistance;
    }

    if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
    {
      robot::log_warning("[recovery_core] '%s': affected_maps='%s' is invalid; using 'both'.",
                         name().c_str(), affected_maps_.c_str());
      affected_maps_ = "both";
    }

    std::vector<std::string> clearable_layers_default{"obstacles"};
    std::vector<std::string> clearable_layers;
    nh.param("layer_names", clearable_layers, clearable_layers_default);
    clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());

    if (clearable_layers_.empty())
    {
      robot::log_error("[recovery_core] '%s': layer_names is empty — this behavior will not clear "
                       "anything.",
                       name().c_str());
      return false;
    }

    const bool needs_global = affected_maps_ == "global" || affected_maps_ == "both";
    const bool needs_local = affected_maps_ == "local" || affected_maps_ == "both";

    if (needs_global && ctx().global_costmap == nullptr)
    {
      robot::log_error("[recovery_core] '%s': affected_maps='%s' but the global costmap is "
                       "missing.",
                       name().c_str(), affected_maps_.c_str());
      return false;
    }
    if (needs_local && ctx().local_costmap == nullptr)
    {
      robot::log_error("[recovery_core] '%s': affected_maps='%s' but the local costmap is missing.",
                       name().c_str(), affected_maps_.c_str());
      return false;
    }

    return true;
  }

  bool onStart(const recovery_core::RecoveryGoal& /*goal*/) override
  {
    // One-shot: công việc thực hiện ở tick đầu tiên, giữ start() không có tác dụng phụ.
    return true;
  }

  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
  {
    bool ok = true;

    if (affected_maps_ == "global" || affected_maps_ == "both")
    {
      ok = clear(ctx().global_costmap, "global") && ok;
    }

    if (affected_maps_ == "local" || affected_maps_ == "both")
    {
      ok = clear(ctx().local_costmap, "local") && ok;
    }

    return ok ? recovery_core::RecoveryResult::Succeeded()
                    .withProgress(1.0, 0.0)
                    .withMessage("clear costmap complete")
              : recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
  }

private:
  bool clear(robot_costmap_2d::Costmap2DROBOT* costmap, const char* which)
  {
    if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
    {
      robot::log_error("[recovery_core] '%s': %s costmap is missing.", name().c_str(), which);
      return false;
    }

    robot_geometry_msgs::PoseStamped pose;
    if (!costmap->getRobotPose(pose))
    {
      robot::log_error("[recovery_core] '%s': could not get the robot pose on the %s costmap.",
                       name().c_str(), which);
      return false;
    }

    std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>* plugins =
        costmap->getLayeredCostmap()->getPlugins();
    if (plugins == nullptr)
    {
      robot::log_error("[recovery_core] '%s': %s costmap has no layer.", name().c_str(),
                       which);
      return false;
    }

    bool touched_layer = false;
    std::string available;

    for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
    {
      if (!plugin)
      {
        continue;
      }

      const std::string layer_name = leafName(plugin->getName());

      if (!available.empty())
      {
        available += ", ";
      }
      available += layer_name;

      if (clearable_layers_.count(layer_name) == 0)
      {
        continue;
      }

      if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
      {
        robot::log_warning("[recovery_core] '%s': layer '%s' is not a CostmapLayer; skipped.",
                           name().c_str(), layer_name.c_str());
        continue;
      }

      clearMap(boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin),
               pose.pose.position.x, pose.pose.position.y);
      touched_layer = true;
    }

    if (!touched_layer)
    {
      // Nguyên nhân phổ biến nhất của "recovery này không làm gì" là sai tên layer trong config.
      // Bản trước trả kFailed lặng lẽ, nên không có cách nào biết vì sao.
      robot::log_error("[recovery_core] '%s': no layer of the %s costmap matches layer_names. "
                       "Layers present: [%s].", name().c_str(), which, available.c_str());
      return false;
    }

    return true;
  }

  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& layer, double pose_x,
                double pose_y)
  {
    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(layer->getMutex()));

    const double start_point_x = pose_x - reset_distance_ / 2.0;
    const double start_point_y = pose_y - reset_distance_ / 2.0;
    const double end_point_x = start_point_x + reset_distance_;
    const double end_point_y = start_point_y + reset_distance_;

    int start_x = 0;
    int start_y = 0;
    int end_x = 0;
    int end_y = 0;
    layer->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
    layer->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);

    layer->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);

    // Báo cho layer biết toàn bộ vùng của nó cần được ghi lại vào master ở chu kỳ update kế tiếp.
    layer->addExtraBounds(layer->getOriginX(), layer->getOriginY(),
                          layer->getOriginX() + layer->getSizeInMetersX(),
                          layer->getOriginY() + layer->getSizeInMetersY());
  }

  double reset_distance_ = kDefaultResetDistance;  ///< [m] cạnh vùng vuông quanh robot
  bool invert_area_to_clear_ = false;              ///< true = xoá phần NGOÀI vùng
  std::string affected_maps_ = "both";             ///< local | global | both
  std::set<std::string> clearable_layers_;
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::ClearCostmapRecovery::create, ClearCostmapRecovery)