/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — path output recovery plugin (goal-driven, one-shot).
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

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

namespace recovery_plugins
{

/**
 * @class RegenPathRecovery
 * @brief Họ A (path output), one-shot: trả lại robot_nav_msgs::Path từ global_path hiện tại.
 *
 * Hoàn tất ngay ở lần update() đầu. Guard chưa configure/global_path null hoặc rỗng -> Failed().
 */
class RegenPathRecovery final : public recovery_core::RecoveryBehavior
{
public:
  RegenPathRecovery() = default;

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

protected:
  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
  {
    // One-shot: công việc thực hiện ở onUpdate() lần đầu, giữ start() gọn.
    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
                                                   recovery_core::RecoveryStatus::kRunning)
        .withMessage("regen path start");
  }

  recovery_core::RecoveryResult onUpdate(double /*dt*/) override
  {
    const auto* global_path = ctx().global_path;
    if (global_path == nullptr || global_path->empty())
    {
      return recovery_core::RecoveryResult::Failed().withMessage("no global path to regenerate");
    }

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

    return recovery_core::RecoveryResult::PathOut(path, recovery_core::RecoveryStatus::kSucceeded)
        .withProgress(1.0, 0.0)
        .withMessage("regen path complete");
  }
};

}  // namespace recovery_plugins

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