/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — đợi tại chỗ, không phát output.
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

#include <cmath>
#include <string>

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

namespace recovery_plugins
{
namespace
{
constexpr double kDefaultWaitDuration = 3.0;  // [s]
constexpr double kMaxWaitDuration = 300.0;    // [s] trần vệ sinh cho param cấu hình sai.
}  // namespace

/**
 * @class WaitRecovery
 * @brief Đứng yên một khoảng thời gian rồi báo thành công.
 *
 * Đây là recovery **an toàn nhất** trong bộ default và nên đứng đầu danh sách: nó không di chuyển,
 * không cần pose, không cần collision check. Với AMR/AGV trong kho, phần lớn tình huống chặn đường
 * là vật cản động (người, xe khác) — đợi vài giây rồi lập plan lại giải quyết được đa số, trong khi
 * mọi behavior khác đều bắt robot cử động trong lúc chưa biết chuyện gì đang xảy ra.
 *
 * Thời lượng: `goal.params["wait_duration"]` cho lượt này, ngược lại param `wait_duration`.
 */
class WaitRecovery final : public recovery_core::RecoveryBehavior
{
public:
  WaitRecovery() = default;

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

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

protected:
  bool onConfigure(robot::NodeHandle& nh) override
  {
    nh.param("wait_duration", default_wait_duration_, kDefaultWaitDuration);
    default_wait_duration_ = sanitizeDuration(default_wait_duration_, kDefaultWaitDuration);
    return true;
  }

  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
    wait_duration_ = sanitizeDuration(goal.param("wait_duration", default_wait_duration_),
                                      default_wait_duration_);
    return true;
  }

  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
  {
    // elapsed() do base đo bằng đồng hồ thật, nên loop chạy chậm không làm sai thời lượng đợi.
    const double waited = elapsed();

    if (waited >= wait_duration_)
    {
      return recovery_core::RecoveryResult::Succeeded()
          .withProgress(1.0, 0.0)
          .withMessage("wait complete");
    }

    return recovery_core::RecoveryResult::Running()
        .withProgress(waited / wait_duration_, wait_duration_ - waited)
        .withMessage("waiting");
  }

private:
  double sanitizeDuration(double value, double fallback) const
  {
    if (!std::isfinite(value) || value <= 0.0 || value > kMaxWaitDuration)
    {
      robot::log_warning("[recovery_core] '%s': wait_duration=%.3f s outside (0, %.0f]; using %.3f "
                         "s.",
                         name().c_str(), value, kMaxWaitDuration, fallback);
      return fallback;
    }
    return value;
  }

  double default_wait_duration_ = kDefaultWaitDuration;  ///< [s]
  double wait_duration_ = kDefaultWaitDuration;          ///< [s] mục tiêu lượt này
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::WaitRecovery::create, WaitRecovery)