/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — path output recovery plugin.
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

#include <string>
#include <vector>

#include <boost/dll/alias.hpp>
#include <robot/robot.h>

namespace recovery_plugins
{

class RegenPathRecovery final : public recovery_core::RecoveryBehavior
{
public:
  RegenPathRecovery() = 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] RegenPathRecovery '%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;

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

  recovery_core::RecoveryResult runBehavior() override
  {
    if (!initialized_ || global_path_ == nullptr || global_path_->empty())
    {
      status_ = recovery_core::RecoveryStatus::kFailed;
      return recovery_core::RecoveryResult::Failed();
    }

    robot_nav_msgs::Path path;
    path.poses = *global_path_;

    status_ = recovery_core::RecoveryStatus::kSucceeded;
    return recovery_core::RecoveryResult::PathOut(path, status_);
  }

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

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

private:
  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;
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)