/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — no-output clear costmap plugin.
 *
 * 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_layer.h>

namespace recovery_plugins
{
namespace
{
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;
}

bool isValidResetDistance(double value)
{
  return std::isfinite(value) && value > 0.0;
}
}  // namespace

class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
{
public:
  ClearCostmapRecovery() = default;

  void initialize(std::string name, tf3::BufferCore* tf,
                  std::vector<robot_geometry_msgs::PoseStamped>* global_path,
                  robot_costmap_2d::Costmap2DROBOT* global_costmap,
                  robot_costmap_2d::Costmap2DROBOT* local_costmap) override
  {
    if (initialized_)
    {
      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' initialized twice; ignoring.",
                       name_.c_str());
      return;
    }

    name_ = std::move(name);
    tf_ = tf;
    global_path_ = global_path;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;

    robot::NodeHandle private_nh("~/" + name_);
    private_nh.param("reset_distance", reset_distance_, 3.0);
    private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
    private_nh.param("force_updating", force_updating_, false);
    private_nh.param("affected_maps", affected_maps_, std::string("both"));

    if (!isValidResetDistance(reset_distance_))
    {
      robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.",
                         name_.c_str());
      reset_distance_ = 3.0;
    }

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

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

    initialized_ = true;
    status_ = recovery_core::RecoveryStatus::kIdle;
  }

  recovery_core::RecoveryResult runBehavior() override
  {
    if (!initialized_)
    {
      status_ = recovery_core::RecoveryStatus::kFailed;
      return recovery_core::RecoveryResult::Failed();
    }

    bool ok = true;
    if (affected_maps_ == "global" || affected_maps_ == "both")
    {
      ok = clear(global_costmap_) && ok;
      if (ok && force_updating_ && global_costmap_ != nullptr)
      {
        global_costmap_->updateMap();
      }
    }

    if (affected_maps_ == "local" || affected_maps_ == "both")
    {
      ok = clear(local_costmap_) && ok;
      if (ok && force_updating_ && local_costmap_ != nullptr)
      {
        local_costmap_->updateMap();
      }
    }

    status_ = ok ? recovery_core::RecoveryStatus::kSucceeded :
                   recovery_core::RecoveryStatus::kFailed;
    return ok ? recovery_core::RecoveryResult::Succeeded() :
                recovery_core::RecoveryResult::Failed();
  }

  recovery_core::RecoveryStatus status() const override
  {
    return status_;
  }

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

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

    robot_geometry_msgs::PoseStamped pose;
    if (!costmap->getRobotPose(pose))
    {
      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' cannot get robot pose.",
                       name_.c_str());
      return false;
    }

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

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

      const std::string name = leafName(plugin->getName());
      if (clearable_layers_.count(name) == 0)
      {
        continue;
      }

      if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
      {
        robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.",
                           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;
    }

    return touched_layer;
  }

  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap,
                double pose_x, double pose_y)
  {
    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->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;
    costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
    costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);

    costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
    costmap->addExtraBounds(costmap->getOriginX(), costmap->getOriginY(),
                            costmap->getOriginX() + costmap->getSizeInMetersX(),
                            costmap->getOriginY() + costmap->getSizeInMetersY());
  }

  tf3::BufferCore* tf_ = nullptr;
  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
  bool initialized_ = false;
  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;

  bool force_updating_ = false;
  double reset_distance_ = 3.0;
  bool invert_area_to_clear_ = false;
  std::string affected_maps_ = "both";
  std::set<std::string> clearable_layers_;
};

}  // namespace recovery_plugins

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