/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — per-cycle rotate 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 kDefaultTargetAngle = 1.57079632679;  // pi/2 rad.
constexpr double kDefaultAngularSpeed = 0.4;           // rad/s.
}  // namespace

/**
 * @class RotateRecovery
 * @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
 *
 * goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ
 * góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả
 * Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command).
 */
class RotateRecovery final : public recovery_core::RecoveryBehavior
{
public:
  RotateRecovery() = default;

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

protected:
  void onConfigure() override
  {
    robot::NodeHandle private_nh("~/" + name_);
    private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle);
    private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);

    if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
    {
      robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
                         name_.c_str());
      default_target_angle_ = kDefaultTargetAngle;
    }
    if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 0.0)
    {
      robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
                         name_.c_str());
      default_angular_speed_ = kDefaultAngularSpeed;
    }
  }

  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
  {
    // Góc đích: goal.angle nếu hợp lệ, ngược lại default configured.
    target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0)
                        ? goal.angle
                        : default_target_angle_;

    angular_speed_ = std::abs(goal.param("angular_speed", default_angular_speed_));
    if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
    {
      angular_speed_ = default_angular_speed_;
    }

    rotated_angle_ = 0.0;
    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
                                                   recovery_core::RecoveryStatus::kRunning)
        .withProgress(0.0, std::abs(target_angle_))
        .withMessage("rotate start");
  }

  recovery_core::RecoveryResult onUpdate(double dt) override
  {
    const double target = std::abs(target_angle_);

    if (rotated_angle_ >= target)
    {
      return succeeded(target);
    }

    robot_geometry_msgs::Twist command;
    command.angular.z = std::copysign(angular_speed_, target_angle_);
    rotated_angle_ = std::min(target, rotated_angle_ + std::abs(command.angular.z) * dt);

    if (rotated_angle_ >= target)
    {
      return succeeded(target);
    }

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

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

  double default_target_angle_ = kDefaultTargetAngle;
  double default_angular_speed_ = kDefaultAngularSpeed;

  double target_angle_ = kDefaultTargetAngle;
  double angular_speed_ = kDefaultAngularSpeed;
  double rotated_angle_ = 0.0;
};

}  // namespace recovery_plugins

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