/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — plugin local planner CHỈ dùng cho test.
 *
 * Instance được nạp qua Boost.DLL nên test không giữ được con trỏ tới nó. Thay vì mở một cửa hậu để
 * đọc trạng thái, planner này **phản ánh** thứ nó nhận được vào chính lệnh vận tốc nó trả về:
 *
 *   cmd.linear.x  = clamp(kBaseSpeed + vận_tốc_đo_được.x, trần_tiến)
 *   cmd.angular.z = clamp(kBaseYawRate, trần_góc)
 *
 * Nhờ vậy "trần đã tới plugin chưa" và "vận tốc đo được đã tới plugin chưa" kiểm được qua đúng API
 * mà runtime dùng, không cần cơ chế quan sát riêng nào.
 *
 * Không alias nào chạm vào con trỏ TF hay costmap — đó là điều kiện để test truyền con trỏ giả thay
 * vì phải dựng `tf3::BufferCore` và `Costmap2DROBOT` thật.
 *
 * Author: DuongTD
 *********************************************************************/
#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <boost/config.hpp>
#include <boost/dll/alias.hpp>

#include <robot_nav_core/base_local_planner.h>

namespace move_base2
{
namespace testing
{

constexpr double kBaseSpeed = 0.25;    ///< [m/s] lệnh nền khi chưa có trần nào
constexpr double kBaseYawRate = 0.40;  ///< [rad/s]

/**
 * @class TestLocalPlanner
 * @brief Local planner giả, hành vi cố định theo tham số dựng.
 */
class TestLocalPlanner : public robot_nav_core::BaseLocalPlanner
{
public:
  enum class Behavior
  {
    kOk,            ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được.
    kNoCommand,     ///< computeVelocityCommands trả false.
    kNaN,           ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
    kThrow,         ///< Ném exception khi tính lệnh.
    kRefusesLimits  ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
  };

  explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
  {
  }

  void initialize(std::string name, tf3::BufferCore* /*tf*/,
                  robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
  {
    // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
    name_ = std::move(name);
  }

  bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
  {
    return !plan.empty();
  }

  void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
  {
    path.clear();
  }

  void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
  {
    path.clear();
  }

  bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity,
                               robot_geometry_msgs::Twist& cmd_vel) override
  {
    switch (behavior_)
    {
      case Behavior::kThrow:
        throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
      case Behavior::kNoCommand:
        return false;
      case Behavior::kNaN:
        cmd_vel.linear.x = std::numeric_limits<double>::quiet_NaN();
        return true;
      case Behavior::kOk:
      case Behavior::kRefusesLimits:
        break;
    }

    double linear = kBaseSpeed + velocity.linear.x;
    if (has_limit_forward_)
    {
      linear = std::min(linear, limit_forward_);
    }

    double yaw = kBaseYawRate;
    if (has_limit_angular_)
    {
      yaw = std::min(yaw, limit_angular_);
    }

    cmd_vel.linear.x = linear;
    cmd_vel.angular.z = yaw;
    return true;
  }

  bool isGoalReached() override
  {
    return false;
  }

  bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override
  {
    if (behavior_ == Behavior::kRefusesLimits)
    {
      return false;
    }
    // Dấu chọn chiều, đúng quy ước của interface gen-1.
    if (linear.x < 0.0)
    {
      limit_backward_ = linear.x;
    }
    else
    {
      limit_forward_ = linear.x;
      has_limit_forward_ = true;
    }
    return true;
  }

  robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override
  {
    robot_geometry_msgs::Vector3 out;
    out.x = direct ? limit_forward_ : limit_backward_;
    return out;
  }

  bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override
  {
    if (behavior_ == Behavior::kRefusesLimits)
    {
      return false;
    }
    limit_angular_ = angular.z;
    has_limit_angular_ = true;
    return true;
  }

  robot_geometry_msgs::Vector3 getTwistAngular(bool /*direct*/) override
  {
    robot_geometry_msgs::Vector3 out;
    out.z = limit_angular_;
    return out;
  }

private:
  Behavior behavior_;
  std::string name_;
  double limit_forward_ = 0.0;   ///< [m/s]
  double limit_backward_ = 0.0;  ///< [m/s], âm
  double limit_angular_ = 0.0;   ///< [rad/s]
  bool has_limit_forward_ = false;
  bool has_limit_angular_ = false;
};

robot_nav_core::BaseLocalPlanner::Ptr createOk()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}

robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}

robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
}

robot_nav_core::BaseLocalPlanner::Ptr createNaN()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
}

robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
}

robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
{
  return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
}

}  // namespace testing
}  // namespace move_base2

BOOST_DLL_ALIAS(move_base2::testing::createOk, TestControllerOk)
BOOST_DLL_ALIAS(move_base2::testing::createSecondary, TestControllerSecondary)
BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)