/*********************************************************************
 * recovery_core — phần chung (template-method) của RecoveryBehavior.
 *
 * Base giữ toàn bộ bất biến vòng đời, thời gian, và họ output; plugin chỉ triển khai hook.
 *
 * Author: DuongTD
 *********************************************************************/
#include <recovery_core/recovery_behavior.h>

#include <cmath>

#include <robot/robot.h>

namespace recovery_core
{
namespace
{
constexpr double kDefaultTimeout = 0.0;   ///< [s] 0 = không giới hạn.
constexpr double kMaxTimeout = 600.0;     ///< [s] trần vệ sinh cho param cấu hình sai.

/// Một giá trị optional hợp lệ phải hữu hạn.
bool finiteIfSet(const std::optional<double>& value)
{
  return !value.has_value() || std::isfinite(*value);
}
}  // namespace

bool RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx,
                                 robot::NodeHandle& nh)
{
  if (configured_)
  {
    // Gọi lại với ctx khác là lỗi lập trình của caller. Bản cũ im lặng return, nên caller không bao
    // giờ biết context thứ hai đã bị bỏ đi.
    robot::log_error("[recovery_core] '%s': configure() called twice, ignored.", name_.c_str());
    return false;
  }

  if (name.empty())
  {
    robot::log_error("[recovery_core] configure() with an empty instance name.");
    return false;
  }

  name_ = name;
  ctx_ = ctx;

  if (!validateContext())
  {
    name_.clear();
    ctx_ = RecoveryContext();
    return false;
  }

  nh.param("timeout", timeout_, kDefaultTimeout);
  if (!std::isfinite(timeout_) || timeout_ < 0.0 || timeout_ > kMaxTimeout)
  {
    robot::log_warning("[recovery_core] '%s': timeout=%.3f s outside [0, %.0f]; using 0 (no "
                       "limit).", name_.c_str(), timeout_, kMaxTimeout);
    timeout_ = kDefaultTimeout;
  }

  if (!onConfigure(nh))
  {
    robot::log_error("[recovery_core] '%s': onConfigure() failed.", name_.c_str());
    name_.clear();
    ctx_ = RecoveryContext();
    return false;
  }

  status_ = RecoveryStatus::kIdle;
  configured_ = true;
  return true;
}

bool RecoveryBehavior::start(const RecoveryGoal& goal, const robot::Time& now)
{
  if (!configured_)
  {
    robot::log_error("[recovery_core] start() before configure().");
    return false;
  }

  if (started_ && status_ == RecoveryStatus::kRunning && !cancel_requested_)
  {
    // Không từ chối: sau cancel(), state machine có thể start lượt mới mà lượt cũ chưa kịp về
    // terminal (nó không tick recovery ở state CANCELLING). Nhưng vẫn phải báo, vì nếu KHÔNG phải
    // đường cancel thì đây là caller đang bỏ dở một behavior đang lái robot.
    robot::log_warning("[recovery_core] '%s': start() while the previous run is still going — "
                       "resetting.",
                       name_.c_str());
  }

  if (!validateGoal(goal))
  {
    return false;
  }

  goal_ = goal;
  cancel_requested_ = false;
  start_time_ = now;
  last_update_ = now;
  elapsed_ = 0.0;
  status_ = RecoveryStatus::kRunning;
  started_ = true;

  if (!onStart(goal_))
  {
    status_ = RecoveryStatus::kFailed;
    robot::log_warning("[recovery_core] '%s': refused to start (trigger=%s).", name_.c_str(),
                       toString(goal_.trigger));
    return false;
  }

  return true;
}

RecoveryResult RecoveryBehavior::update(const robot::Time& now)
{
  if (!configured_ || !started_)
  {
    status_ = RecoveryStatus::kFailed;
    return stopResult(RecoveryStatus::kFailed).withMessage("update() before start()");
  }

  if (status_ != RecoveryStatus::kRunning)
  {
    // Lượt đã kết thúc: giữ nguyên kết luận, không tick thêm.
    return stopResult(status_);
  }

  // dt đo THẬT. Đây là điểm sửa cốt lõi so với bản cũ: bản cũ tích phân vận tốc lệnh nhân với
  // control_period lấy từ config, nên control loop chạy chậm là robot đi quá quãng yêu cầu.
  double dt = (now - last_update_).toSec();
  if (!std::isfinite(dt) || dt < 0.0)
  {
    // Đồng hồ đi lùi (đổi nguồn thời gian, hoặc sim reset). Coi như không có thời gian trôi thay vì
    // tích phân một dt âm vào tiến độ.
    robot::log_warning("[recovery_core] '%s': dt=%.6f s is invalid, treated as 0.", name_.c_str(),
                       dt);
    dt = 0.0;
  }
  last_update_ = now;
  elapsed_ = (now - start_time_).toSec();
  if (!std::isfinite(elapsed_) || elapsed_ < 0.0)
  {
    elapsed_ = 0.0;
  }

  if (cancel_requested_)
  {
    RecoveryResult result = finalize(onCancel());
    status_ = result.status;
    return result;
  }

  if (timeout_ > 0.0 && elapsed_ >= timeout_)
  {
    status_ = RecoveryStatus::kFailed;
    return stopResult(RecoveryStatus::kFailed)
        .withMessage("exceeded the timeout of " + std::to_string(timeout_) + " s");
  }

  RecoveryResult result = finalize(onUpdate(now, dt));
  status_ = result.status;
  return result;
}

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

RecoveryResult RecoveryBehavior::onCancel()
{
  return stopResult(RecoveryStatus::kCancelled).withMessage("cancelled by caller");
}

RecoveryResult RecoveryBehavior::stopResult(RecoveryStatus status) const
{
  RecoveryResult result;
  result.status = status;
  result.elapsed = elapsed_;

  if (outputKind() == RecoveryOutputType::kVelocity)
  {
    // Họ velocity: phát lệnh dừng TƯỜNG MINH. Caller đang lấy cmd_vel từ behavior này nên "không
    // output" và "output vận tốc 0" là hai chuyện khác nhau.
    result.output_type = RecoveryOutputType::kVelocity;
    result.command = robot_geometry_msgs::Twist();
  }
  else
  {
    // Họ khác: KHÔNG bịa ra output vận tốc. Bản cũ trả Velocity(zero) cho mọi họ, nên một behavior
    // clear-costmap báo cáo mình phát vận tốc.
    result.output_type = RecoveryOutputType::kNone;
  }

  return result;
}

bool RecoveryBehavior::validateContext() const
{
  const RecoveryOutputType kind = outputKind();

  if (kind == RecoveryOutputType::kVelocity)
  {
    if (ctx_.pose == nullptr)
    {
      robot::log_error("[recovery_core] '%s': the velocity family requires a PoseProvider — "
                       "progress must be measured from a real pose, not dead-reckoned.",
                       name_.c_str());
      return false;
    }
    if (ctx_.collision == nullptr)
    {
      robot::log_error("[recovery_core] '%s': the velocity family requires a CollisionChecker — "
                       "driving blind is not allowed.", name_.c_str());
      return false;
    }
  }

  if (kind == RecoveryOutputType::kPath && ctx_.plan == nullptr)
  {
    robot::log_error("[recovery_core] '%s': the path family requires a PlanProvider.",
                     name_.c_str());
    return false;
  }

  return true;
}

bool RecoveryBehavior::validateGoal(const RecoveryGoal& goal) const
{
  if (!finiteIfSet(goal.angle) || !finiteIfSet(goal.distance))
  {
    robot::log_error("[recovery_core] '%s': goal contains NaN/Inf.", name_.c_str());
    return false;
  }

  if (goal.distance.has_value() && *goal.distance <= 0.0)
  {
    robot::log_error("[recovery_core] '%s': goal.distance=%.3f m must be > 0.", name_.c_str(),
                     *goal.distance);
    return false;
  }

  for (const auto& entry : goal.params)
  {
    if (!std::isfinite(entry.second))
    {
      robot::log_error("[recovery_core] '%s': goal.params['%s'] is not finite.", name_.c_str(),
                       entry.first.c_str());
      return false;
    }
  }

  return true;
}

RecoveryResult RecoveryBehavior::finalize(RecoveryResult result) const
{
  const RecoveryOutputType kind = outputKind();

  if (result.output_type != kind && result.output_type != RecoveryOutputType::kNone)
  {
    // Plugin trả sai họ. Hạ về kNone thay vì tin theo: caller route bằng output_type, nên một họ
    // sai ở đây là caller đọc nhầm trường.
    robot::log_error("[recovery_core] '%s': returned output_type='%s' but outputKind()='%s'; "
                     "downgraded to 'none'.", name_.c_str(), toString(result.output_type), toString(kind));
    result.output_type = RecoveryOutputType::kNone;
    result.command = robot_geometry_msgs::Twist();
    result.path = robot_nav_msgs::Path();
  }

  if (result.output_type == RecoveryOutputType::kVelocity)
  {
    const robot_geometry_msgs::Twist& cmd = result.command;
    if (!std::isfinite(cmd.linear.x) || !std::isfinite(cmd.linear.y) ||
        !std::isfinite(cmd.linear.z) || !std::isfinite(cmd.angular.x) ||
        !std::isfinite(cmd.angular.y) || !std::isfinite(cmd.angular.z))
    {
      // NaN/Inf lọt ra cmd_vel là lỗi không được phép đi tiếp: đổi thành lệnh dừng + kFailed.
      robot::log_error("[recovery_core] '%s': velocity command contains NaN/Inf — forcing a stop.",
                       name_.c_str());
      RecoveryResult stop = stopResult(RecoveryStatus::kFailed);
      stop.message = "velocity command is not finite";
      return stop;
    }
  }

  result.elapsed = elapsed_;
  return result;
}

}  // namespace recovery_core