/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — quay tại chỗ, quét cung trước khi quay và đo bằng pose thật.
 *
 * 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 kTwoPi = 2.0 * M_PI;
constexpr double kDefaultAngularSpeed = 0.4;    // [rad/s] độ lớn
constexpr double kDefaultAccLimTheta = 0.8;     // [rad/s^2]
constexpr double kDefaultSimGranularity = 0.1;  // [rad] bước quét cung
constexpr double kMaxAngularSpeed = 2.0;        // [rad/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3;         // [rad]
}  // namespace

/**
 * @class RotateRecovery
 * @brief Quay tại chỗ, mặc định đủ một vòng, để costmap quan sát lại xung quanh.
 *
 * Quay đủ 2π là công dụng chính của rotate trong một bộ recovery: nó cho obstacle/voxel layer nhìn
 * thấy toàn bộ vùng quanh robot rồi mới lập plan lại. Đặt `full_rotation: false` hoặc truyền
 * `goal.angle` để quay một góc cụ thể.
 *
 * Hai lớp bảo vệ so với bản trước (bản trước không dùng `ctx()` một lần nào):
 *  1. **Quét toàn bộ cung sẽ quay** tại `onStart()` theo bước `sim_granularity`; chạm vật cản ở bất
 *     kỳ góc nào là từ chối khởi động, chứ không quay tới nơi mới phát hiện.
 *  2. **Tiến độ đo bằng pose thật**, cộng dồn từng chênh lệch yaw đã chuẩn hoá — nên quay > π vẫn
 *     đếm đúng, và loop chạy chậm không làm robot quay quá góc.
 *
 * @note Yêu cầu robot xoay tại chỗ được (differential/omni). Config workspace hiện tại thoả:
 *       `min_turn_radius: 0.0`, `use_rotate_to_heading: true`.
 */
class RotateRecovery final : public recovery_core::RecoveryBehavior
{
public:
  RotateRecovery() = default;

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

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

protected:
  bool onConfigure(robot::NodeHandle& nh) override
  {
    nh.param("full_rotation", full_rotation_, true);
    nh.param("target_angle", default_target_angle_, kTwoPi);
    nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
    nh.param("acc_lim_theta", acc_lim_theta_, kDefaultAccLimTheta);
    nh.param("sim_granularity", sim_granularity_, kDefaultSimGranularity);

    default_target_angle_ = clampAngle(default_target_angle_, kTwoPi);
    default_angular_speed_ = clampSpeed(default_angular_speed_, kDefaultAngularSpeed);

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

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

    return true;
  }

  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
    robot_geometry_msgs::PoseStamped pose;
    if (!ctx().pose->getRobotPose(pose))
    {
      robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
                         name().c_str());
      return false;
    }

    start_pose_ = pose;
    start_yaw_ = recovery_core::yawOf(pose);
    last_yaw_ = start_yaw_;
    rotated_ = 0.0;
    current_speed_ = 0.0;

    // goal.angle có giá trị (kể cả 0.0) thì tôn trọng đúng giá trị đó. Bản trước coi 0 là "chưa
    // đặt" nên một góc tính ra ~0 bị âm thầm thay bằng pi/2.
    const double requested = goal.angle.value_or(full_rotation_ ? kTwoPi : default_target_angle_);
    target_angle_ = clampAngle(requested, default_target_angle_);
    angular_speed_ = clampSpeed(std::abs(goal.param("angular_speed", default_angular_speed_)),
                                default_angular_speed_);

    if (std::abs(target_angle_) <= kGoalTolerance)
    {
      // Caller nói rõ "đừng quay". Đó là một yêu cầu hợp lệ và hoàn tất ngay.
      zero_rotation_ = true;
      return true;
    }
    zero_rotation_ = false;

    if (!arcIsClear())
    {
      robot::log_warning("[recovery_core] '%s': the %.3f rad arc is blocked, refusing to rotate.",
                         name().c_str(), target_angle_);
      return false;
    }

    return true;
  }

  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
  {
    const double target = std::abs(target_angle_);

    if (zero_rotation_)
    {
      return stopResult(recovery_core::RecoveryStatus::kSucceeded)
          .withProgress(1.0, 0.0)
          .withMessage("rotate 0 rad — nothing to rotate");
    }

    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 rotation");
    }

    const double yaw = recovery_core::yawOf(pose);

    // Cộng dồn từng bước đã chuẩn hoá: cách duy nhất đếm đúng khi tổng góc quay vượt pi.
    rotated_ += std::abs(recovery_core::normalizeAngle(yaw - last_yaw_));
    last_yaw_ = yaw;

    const double remaining = target - rotated_;
    if (remaining <= kGoalTolerance)
    {
      current_speed_ = 0.0;
      return stopResult(recovery_core::RecoveryStatus::kSucceeded)
          .withProgress(1.0, 0.0)
          .withMessage("rotate complete");
    }

    double speed = recovery_core::rampToward(angular_speed_, current_speed_, acc_lim_theta_, dt);
    if (dt > 0.0)
    {
      speed = std::min(speed, remaining / dt);
    }
    speed = std::max(speed, 0.0);

    // Cung đã quét ở onStart(), nhưng costmap đổi giữa chừng thì phải phát hiện: kiểm góc dự đoán
    // ở cuối chu kỳ tới trước khi phát lệnh.
    const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
    const double next_yaw = yaw + direction * std::max(speed * dt, sim_granularity_);
    if (ctx().collision->footprintCost(pose.pose.position.x, pose.pose.position.y, next_yaw) < 0.0)
    {
      current_speed_ = 0.0;
      return stopResult(recovery_core::RecoveryStatus::kFailed)
          .withMessage("the rotation arc became blocked midway — stopping the rotation");
    }

    current_speed_ = speed;

    robot_geometry_msgs::Twist command;
    command.angular.z = std::copysign(speed, target_angle_);  // [rad/s], + = ngược chiều kim đồng hồ

    return recovery_core::RecoveryResult::Velocity(command,
                                                   recovery_core::RecoveryStatus::kRunning)
        .withProgress(rotated_ / target, remaining)
        .withMessage("rotating");
  }

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

private:
  /// @return true nếu toàn bộ cung sẽ quay đều đặt được footprint.
  bool arcIsClear() const
  {
    const double target = std::abs(target_angle_);
    const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
    const double x = start_pose_.pose.position.x;
    const double y = start_pose_.pose.position.y;

    for (double swept = 0.0; swept < target; swept += sim_granularity_)
    {
      if (ctx().collision->footprintCost(x, y, start_yaw_ + direction * swept) < 0.0)
      {
        return false;
      }
    }

    // Kiểm luôn góc cuối: vòng lặp trên dừng trước target nếu target không chia hết cho bước quét.
    return ctx().collision->footprintCost(x, y, start_yaw_ + direction * target) >= 0.0;
  }

  double clampAngle(double value, double fallback) const
  {
    if (!std::isfinite(value))
    {
      robot::log_warning("[recovery_core] '%s': target_angle is not finite; using %.3f rad.",
                         name().c_str(), fallback);
      return fallback;
    }
    if (std::abs(value) > kTwoPi)
    {
      robot::log_warning("[recovery_core] '%s': target_angle=%.3f rad exceeds +/-2pi; clamped.",
                         name().c_str(), value);
      return std::copysign(kTwoPi, value);
    }
    return value;
  }

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

  // Config
  bool full_rotation_ = true;
  double default_target_angle_ = kTwoPi;                  ///< [rad]
  double default_angular_speed_ = kDefaultAngularSpeed;   ///< [rad/s]
  double acc_lim_theta_ = kDefaultAccLimTheta;            ///< [rad/s^2]
  double sim_granularity_ = kDefaultSimGranularity;       ///< [rad]

  // Trạng thái lượt hiện tại
  robot_geometry_msgs::PoseStamped start_pose_;
  double start_yaw_ = 0.0;                                ///< [rad]
  double last_yaw_ = 0.0;                                 ///< [rad]
  double rotated_ = 0.0;                                  ///< [rad] cộng dồn, luôn >= 0
  double target_angle_ = kTwoPi;                          ///< [rad] có dấu
  double angular_speed_ = kDefaultAngularSpeed;           ///< [rad/s] độ lớn
  double current_speed_ = 0.0;                            ///< [rad/s] đang phát, để ramp
  bool zero_rotation_ = false;                            ///< caller yêu cầu góc 0
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, RotateRecovery)