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

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

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

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

namespace recovery_plugins
{
namespace
{
robot_geometry_msgs::Twist zeroTwist()
{
  return robot_geometry_msgs::Twist();
}

bool validCycle(double dt)
{
  return std::isfinite(dt) && dt > 0.0;
}
}  // namespace

class RotateRecovery final : public recovery_core::RecoveryBehavior
{
public:
  RotateRecovery() = default;

  void initialize(std::string name, tf3::BufferCore* tf,
                  std::vector<robot_geometry_msgs::PoseStamped>* global_path,
                  robot_costmap_2d::Costmap2DROBOT* global_costmap,
                  robot_costmap_2d::Costmap2DROBOT* local_costmap) override
  {
    if (initialized_)
    {
      robot::log_error("[recovery_core] RotateRecovery '%s' initialized twice; ignoring.",
                       name_.c_str());
      return;
    }

    name_ = std::move(name);
    tf_ = tf;
    global_path_ = global_path;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;

    robot::NodeHandle private_nh("~/" + name_);
    config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
    private_nh.param("target_angle", target_angle_, 1.57079632679);
    private_nh.param("angular_speed", angular_speed_, 0.4);

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

    initialized_ = true;
    status_ = recovery_core::RecoveryStatus::kIdle;
  }

  recovery_core::RecoveryResult runBehavior() override
  {
    status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
                             recovery_core::RecoveryStatus::kFailed;
    return initialized_ ? recovery_core::RecoveryResult::Running() :
                          recovery_core::RecoveryResult::Failed();
  }

  recovery_core::RecoveryResult computeCommand(double dt) override
  {
    if (!initialized_ || !validCycle(dt))
    {
      status_ = recovery_core::RecoveryStatus::kFailed;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
    }

    elapsed_ += dt;
    if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
    {
      status_ = recovery_core::RecoveryStatus::kFailed;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
    }

    const double target = std::abs(target_angle_);
    if (rotated_angle_ >= target)
    {
      status_ = recovery_core::RecoveryStatus::kSucceeded;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
    }

    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)
    {
      status_ = recovery_core::RecoveryStatus::kSucceeded;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
    }

    status_ = recovery_core::RecoveryStatus::kRunning;
    return recovery_core::RecoveryResult::Velocity(command, status_);
  }

  recovery_core::RecoveryStatus status() const override
  {
    return status_;
  }

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

private:
  tf3::BufferCore* tf_ = nullptr;
  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
  recovery_core::RecoveryConfig config_;
  bool initialized_ = false;
  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;

  double target_angle_ = 1.57079632679;
  double angular_speed_ = 0.4;
  double elapsed_ = 0.0;
  double rotated_angle_ = 0.0;
};

}  // namespace recovery_plugins

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