/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — per-cycle backup recovery plugin (goal-driven).
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>

#include <algorithm>
#include <cmath>
#include <string>

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

namespace recovery_plugins
{
namespace
{
constexpr double kDefaultBackupDistance = 0.5;  // m.
constexpr double kDefaultLinearSpeed = 0.1;     // m/s.
constexpr double kDefaultControlPeriod = 0.1;   // s per update tick.
}  // namespace

/**
 * @class BackUpRecovery
 * @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal).
 *
 * goal.distance (m, > 0) là khoảng lùi lượt này; 0 nghĩa là dùng default configured. Tốc độ
 * tuyến tính mặc định đọc từ param, có thể override qua goal.params["linear_speed"]. Mỗi
 * update() trả Twist.linear.x < 0 kèm progress/remaining tới khi đủ khoảng -> kSucceeded.
 */
class BackUpRecovery final : public recovery_core::RecoveryBehavior
{
public:
  BackUpRecovery() = default;

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

protected:
  void onConfigure() override
  {
    robot::NodeHandle private_nh("~/" + name_);
    private_nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
    private_nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
    private_nh.param("control_period", control_period_, kDefaultControlPeriod);
    private_nh.param("require_costmap", require_costmap_, false);

    if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0)
    {
      robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
                         name_.c_str());
      default_backup_distance_ = kDefaultBackupDistance;
    }
    if (!std::isfinite(default_linear_speed_) || default_linear_speed_ <= 0.0)
    {
      robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
                         name_.c_str());
      default_linear_speed_ = kDefaultLinearSpeed;
    }
    if (!std::isfinite(control_period_) || control_period_ <= 0.0)
    {
      robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
                         name_.c_str());
      control_period_ = kDefaultControlPeriod;
    }
  }

  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
  {
    // Costmap là bắt buộc? fail sớm trước khi xuất vận tốc lùi.
    if (require_costmap_ && ctx().local_costmap == nullptr)
    {
      return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap");
    }

    backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0)
                           ? goal.distance
                           : default_backup_distance_;

    linear_speed_ = std::abs(goal.param("linear_speed", default_linear_speed_));
    if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
    {
      linear_speed_ = default_linear_speed_;
    }

    traveled_distance_ = 0.0;
    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
                                                   recovery_core::RecoveryStatus::kRunning)
        .withProgress(0.0, backup_distance_)
        .withMessage("backup start");
  }

  recovery_core::RecoveryResult onUpdate() override
  {
    if (traveled_distance_ >= backup_distance_)
    {
      return succeeded();
    }

    robot_geometry_msgs::Twist command;
    command.linear.x = -std::abs(linear_speed_);
    traveled_distance_ = std::min(
        backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_);

    if (traveled_distance_ >= backup_distance_)
    {
      return succeeded();
    }

    return recovery_core::RecoveryResult::Velocity(command,
                                                   recovery_core::RecoveryStatus::kRunning)
        .withProgress(traveled_distance_ / backup_distance_,
                      backup_distance_ - traveled_distance_)
        .withMessage("backing up");
  }

private:
  recovery_core::RecoveryResult succeeded()
  {
    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
                                                   recovery_core::RecoveryStatus::kSucceeded)
        .withProgress(1.0, 0.0)
        .withMessage("backup complete");
  }

  double default_backup_distance_ = kDefaultBackupDistance;
  double default_linear_speed_ = kDefaultLinearSpeed;
  double control_period_ = kDefaultControlPeriod;
  bool require_costmap_ = false;

  double backup_distance_ = kDefaultBackupDistance;
  double linear_speed_ = kDefaultLinearSpeed;
  double traveled_distance_ = 0.0;
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::BackUpRecovery::create, BackUpRecovery)