/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — lùi thẳng một quãng, đo bằng pose thật và có kiểm va chạm.
 *
 * Author: DuongTD
 *********************************************************************/

#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>

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

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

namespace recovery_plugins
{
namespace
{
constexpr double kDefaultBackupDistance = 0.3;      // [m]
constexpr double kDefaultBackupDistanceMax = 1.0;   // [m] trần cứng cho quãng lùi
constexpr double kDefaultLinearSpeed = 0.1;         // [m/s] độ lớn
constexpr double kDefaultAccLimX = 0.3;             // [m/s^2]
constexpr double kMaxLinearSpeed = 1.0;             // [m/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3;             // [m]
}  // namespace

/**
 * @class BackUpRecovery
 * @brief Lùi thẳng theo hướng ban đầu tới khi đủ quãng yêu cầu.
 *
 * Đây là behavior **nguy hiểm nhất** trong bộ default: lùi là hướng robot thường không có sensor.
 * Vì vậy nó xếp cuối danh sách, và có ba lớp bảo vệ:
 *
 *  1. **Tiến độ đo bằng pose thật** — hình chiếu delta pose lên hướng xuất phát, không tích phân
 *     vận tốc lệnh. Bản trước nhân vận tốc lệnh với `control_period` lấy từ config, nên control
 *     loop chạy chậm gấp đôi là robot lùi gấp đôi quãng yêu cầu, còn bánh trượt thì vẫn báo xong.
 *  2. **Kiểm va chạm mỗi tick** trên pose dự đoán ở cuối chu kỳ tới, trước khi phát lệnh; và một
 *     lần nữa lúc `onStart()` để không bao giờ khởi động vào chỗ đã bị chặn.
 *  3. **Mất pose là dừng** — `PoseProvider` trả false thì trả `kFailed` + Twist 0.
 *
 * Quãng lùi: `goal.distance` cho lượt này, ngược lại param `backup_distance`; cả hai bị kẹp bởi
 * `backup_distance_max`.
 */
class BackUpRecovery final : public recovery_core::RecoveryBehavior
{
public:
  BackUpRecovery() = default;

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

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

protected:
  bool onConfigure(robot::NodeHandle& nh) override
  {
    nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
    nh.param("backup_distance_max", backup_distance_max_, kDefaultBackupDistanceMax);
    nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
    nh.param("acc_lim_x", acc_lim_x_, kDefaultAccLimX);

    if (!std::isfinite(backup_distance_max_) || backup_distance_max_ <= 0.0)
    {
      robot::log_warning("[recovery_core] '%s': backup_distance_max=%.3f m is invalid; using %.3f "
                         "m.", name().c_str(), backup_distance_max_,
                         kDefaultBackupDistanceMax);
      backup_distance_max_ = kDefaultBackupDistanceMax;
    }

    default_backup_distance_ = clampDistance(default_backup_distance_, kDefaultBackupDistance);
    default_linear_speed_ = clampSpeed(default_linear_speed_, kDefaultLinearSpeed);

    if (!std::isfinite(acc_lim_x_) || acc_lim_x_ < 0.0)
    {
      robot::log_warning("[recovery_core] '%s': acc_lim_x=%.3f m/s^2 is invalid; using %.3f.",
                         name().c_str(), acc_lim_x_, kDefaultAccLimX);
      acc_lim_x_ = kDefaultAccLimX;
    }

    return true;
  }

  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
    // Base đã bảo đảm ctx().pose và ctx().collision khác null cho họ velocity.
    if (!ctx().pose->getRobotPose(start_pose_))
    {
      robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
                         name().c_str());
      return false;
    }

    start_yaw_ = recovery_core::yawOf(start_pose_);

    backup_distance_ = clampDistance(goal.distance.value_or(default_backup_distance_),
                                     default_backup_distance_);
    linear_speed_ = clampSpeed(std::abs(goal.param("linear_speed", default_linear_speed_)),
                               default_linear_speed_);

    current_speed_ = 0.0;

    // Kiểm ngay tại chỗ: nếu vị trí lùi đầu tiên đã bị chặn thì từ chối khởi động, để state machine
    // chuyển sang behavior kế tiếp thay vì phát một lệnh lùi rồi mới hỏng.
    if (blockedAhead(start_pose_, kProbeDistance))
    {
      robot::log_warning("[recovery_core] '%s': the space behind is already blocked, refusing to "
                         "back up.",
                         name().c_str());
      return false;
    }

    return true;
  }

  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
  {
    robot_geometry_msgs::PoseStamped pose;
    if (!ctx().pose->getRobotPose(pose))
    {
      current_speed_ = 0.0;
      return stopResult(recovery_core::RecoveryStatus::kFailed)
          .withMessage("robot pose lost (TF stale?) — stopping the back-up");
    }

    // Tiến độ = phần đi ngược hướng xuất phát. Dấu âm của hình chiếu chính là quãng đã lùi.
    const double traveled = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
    const double remaining = backup_distance_ - traveled;

    if (remaining <= kGoalTolerance)
    {
      current_speed_ = 0.0;
      return stopResult(recovery_core::RecoveryStatus::kSucceeded)
          .withProgress(1.0, 0.0)
          .withMessage("backup complete");
    }

    // Vận tốc của tick này, đã ramp theo trần gia tốc, và không vượt quãng còn lại nếu dt cho phép.
    double speed = recovery_core::rampToward(linear_speed_, current_speed_, acc_lim_x_, dt);
    if (dt > 0.0)
    {
      speed = std::min(speed, remaining / dt);
    }
    speed = std::max(speed, 0.0);

    // Pose dự đoán ở CUỐI chu kỳ tới — kiểm trước khi phát lệnh, không phải sau.
    const double probe = std::max(speed * std::max(dt, kMinProbeDt), kProbeDistance);
    if (blockedAhead(pose, probe))
    {
      current_speed_ = 0.0;
      return stopResult(recovery_core::RecoveryStatus::kFailed)
          .withMessage("obstacle behind — cancelling the back-up");
    }

    current_speed_ = speed;

    robot_geometry_msgs::Twist command;
    command.linear.x = -speed;  // [m/s], âm = lùi

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

  recovery_core::RecoveryResult onCancel() override
  {
    current_speed_ = 0.0;
    return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("backup cancelled");
  }

private:
  /// Khoảng dò tối thiểu [m] — luôn nhìn trước ít nhất một ô costmap kể cả khi dt rất nhỏ.
  static constexpr double kProbeDistance = 0.05;
  /// dt tối thiểu [s] dùng khi dự đoán, để tick đầu (dt = 0) vẫn dò về phía trước.
  static constexpr double kMinProbeDt = 0.1;

  /// @return true nếu đặt robot lùi thêm @p distance từ @p from là va chạm.
  bool blockedAhead(const robot_geometry_msgs::PoseStamped& from, double distance) const
  {
    const double next_x = from.pose.position.x - std::cos(start_yaw_) * distance;
    const double next_y = from.pose.position.y - std::sin(start_yaw_) * distance;
    return ctx().collision->footprintCost(next_x, next_y, start_yaw_) < 0.0;
  }

  double clampDistance(double value, double fallback) const
  {
    if (!std::isfinite(value) || value <= 0.0)
    {
      robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m is invalid; using %.3f m.",
                         name().c_str(), value, fallback);
      return std::min(fallback, backup_distance_max_);
    }
    if (value > backup_distance_max_)
    {
      robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m > limit %.3f m; clamped.",
                         name().c_str(), value, backup_distance_max_);
      return backup_distance_max_;
    }
    return value;
  }

  double clampSpeed(double value, double fallback) const
  {
    if (!std::isfinite(value) || value <= 0.0)
    {
      robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s is invalid; using %.3f m/s.",
                         name().c_str(), value, fallback);
      return fallback;
    }
    if (value > kMaxLinearSpeed)
    {
      robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s > limit %.3f m/s; clamped.",
                         name().c_str(), value, kMaxLinearSpeed);
      return kMaxLinearSpeed;
    }
    return value;
  }

  // Config
  double default_backup_distance_ = kDefaultBackupDistance;    ///< [m]
  double backup_distance_max_ = kDefaultBackupDistanceMax;     ///< [m]
  double default_linear_speed_ = kDefaultLinearSpeed;          ///< [m/s]
  double acc_lim_x_ = kDefaultAccLimX;                         ///< [m/s^2]

  // Trạng thái lượt hiện tại
  robot_geometry_msgs::PoseStamped start_pose_;
  double start_yaw_ = 0.0;                                     ///< [rad]
  double backup_distance_ = kDefaultBackupDistance;            ///< [m]
  double linear_speed_ = kDefaultLinearSpeed;                  ///< [m/s]
  double current_speed_ = 0.0;                                 ///< [m/s] đang phát, để ramp
};

}  // namespace recovery_plugins

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