/*********************************************************************
 * recovery_core — phần chung (template-method) của RecoveryBehavior.
 *
 * configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời và xử lý
 * cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
 *
 * Author: DuongTD
 *********************************************************************/
#include <recovery_core/recovery_behavior.h>

namespace recovery_core
{
namespace
{
robot_geometry_msgs::Twist zeroTwist()
{
  return robot_geometry_msgs::Twist();
}
}  // namespace

void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx)
{
  if (configured_)
  {
    return;
  }

  name_ = name;
  ctx_ = ctx;
  status_ = RecoveryStatus::kIdle;

  onConfigure();

  configured_ = true;
}

RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal)
{
  if (!configured_)
  {
    status_ = RecoveryStatus::kFailed;
    return RecoveryResult::Failed().withMessage("start() before configure()");
  }

  goal_ = goal;
  cancel_requested_ = false;
  started_ = true;
  status_ = RecoveryStatus::kRunning;

  RecoveryResult result = onStart(goal_);
  status_ = result.status;
  return result;
}

RecoveryResult RecoveryBehavior::update()
{
  if (!configured_ || !started_)
  {
    status_ = RecoveryStatus::kFailed;
    return RecoveryResult::Failed().withMessage("update() before start()");
  }

  // Đã kết thúc: giữ nguyên trạng thái, không tick thêm.
  if (status_ != RecoveryStatus::kRunning)
  {
    return RecoveryResult::Velocity(zeroTwist(), status_);
  }

  if (cancel_requested_)
  {
    status_ = RecoveryStatus::kCancelled;
    return RecoveryResult::Velocity(zeroTwist(), status_)
        .withMessage("cancelled by caller");
  }

  RecoveryResult result = onUpdate();
  status_ = result.status;
  return result;
}

void RecoveryBehavior::cancel()
{
  cancel_requested_ = true;
}

}  // namespace recovery_core