/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — plugin global planner CHỈ dùng cho test.
 *
 * Bốn alias được export từ cùng một thư viện, mỗi alias là một hành vi mà `PlannerRunner` phải xử lý
 * đúng. Chọn cách này thay vì một plugin đọc config: nó làm test không phụ thuộc vào cây config, và
 * mỗi kịch bản gọi tên đúng thứ nó kiểm.
 *
 * Không alias nào chạm vào con trỏ costmap — đó là điều kiện để test truyền vào một con trỏ giả
 * thay vì phải dựng `Costmap2DROBOT` thật (cần tf3::BufferCore và cây config đầy đủ).
 *
 * Author: DuongTD
 *********************************************************************/
#include <stdexcept>
#include <string>
#include <vector>

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

#include <robot_nav_core/base_global_planner.h>

namespace move_base2
{
namespace testing
{

/// @brief Số pose trong plan mà `TestPlannerOk` sinh ra.
constexpr std::size_t kPlanLength = 3;

/**
 * @class TestGlobalPlanner
 * @brief Planner giả, hành vi cố định theo tham số dựng.
 */
class TestGlobalPlanner : public robot_nav_core::BaseGlobalPlanner
{
public:
  enum class Behavior
  {
    kOk,          ///< Trả plan hợp lệ.
    kEmptyPlan,   ///< Trả true kèm plan RỖNG — bẫy mà PlannerRunner phải quy về false.
    kThrow,       ///< Ném exception giữa lúc lập plan.
    kInitFails    ///< initialize() trả false.
  };

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

  bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
  {
    // Cố ý KHÔNG chạm costmap_robot — xem chú thích đầu file.
    name_ = std::move(name);
    return behavior_ != Behavior::kInitFails;
  }

  bool makePlan(const robot_geometry_msgs::PoseStamped& start,
                const robot_geometry_msgs::PoseStamped& goal,
                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
  {
    ++call_count_;

    if (behavior_ == Behavior::kThrow)
    {
      throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
    }

    plan.clear();
    if (behavior_ == Behavior::kEmptyPlan)
    {
      return true;  // true + rỗng: đúng thứ contract PlannerPort cấm lọt qua.
    }

    plan.push_back(start);
    for (std::size_t i = plan.size(); i + 1 < kPlanLength; ++i)
    {
      plan.push_back(start);
    }
    plan.push_back(goal);
    return true;
  }

  bool makePlan(const robot_protocol_msgs::Order& /*msg*/,
                const robot_geometry_msgs::PoseStamped& start,
                const robot_geometry_msgs::PoseStamped& goal,
                std::vector<robot_geometry_msgs::PoseStamped>& plan) override
  {
    saw_order_ = true;
    return makePlan(start, goal, plan);
  }

private:
  Behavior behavior_;
  std::string name_;
  std::size_t call_count_ = 0;
  bool saw_order_ = false;
};

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

robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan()
{
  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kEmptyPlan);
}

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

robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing()
{
  return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kInitFails);
}

}  // namespace testing
}  // namespace move_base2

BOOST_DLL_ALIAS(move_base2::testing::createOk, TestPlannerOk)
BOOST_DLL_ALIAS(move_base2::testing::createEmptyPlan, TestPlannerEmptyPlan)
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestPlannerThrowing)
BOOST_DLL_ALIAS(move_base2::testing::createInitFailing, TestPlannerInitFails)