/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — per-cycle backup 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 BackUpRecovery final : public recovery_core::RecoveryBehavior
{
public:
  BackUpRecovery() = 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] BackUpRecovery '%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("backup_distance", backup_distance_, 0.5);
    private_nh.param("linear_speed", linear_speed_, 0.1);
    private_nh.param("require_costmap", require_costmap_, false);

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

    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) || (require_costmap_ && local_costmap_ == nullptr))
    {
      status_ = recovery_core::RecoveryStatus::kFailed;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(),  );
    }

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

    if (traveled_distance_ >= backup_distance_)
    {
      status_ = recovery_core::RecoveryStatus::kSucceeded;
      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
    }

    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) * dt);

    if (traveled_distance_ >= backup_distance_)
    {
      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<BackUpRecovery>();
  }

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 backup_distance_ = 0.5;
  double linear_speed_ = 0.1;
  bool require_costmap_ = false;
  double elapsed_ = 0.0;
  double traveled_distance_ = 0.0;
};

}  // namespace recovery_plugins

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