/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — layer gián điệp dùng chung cho các test đường vào cảm biến.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MOVE_BASE2_TEST_SPY_LAYER_H_
#define MOVE_BASE2_TEST_SPY_LAYER_H_

#include <cstddef>
#include <functional>
#include <stdexcept>
#include <string>
#include <typeinfo>
#include <vector>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>

namespace move_base2
{
namespace testing
{

/**
 * @class SpyLayer
 * @brief Layer chỉ ghi lại những gì nó nhận được.
 *
 * Dùng với `LayeredCostmap` **thật**: ba contract ẩn cần khoá lại (type erasure qua `void*` +
 * `type_info`, `name` là khoá topic, bộ lọc chọn layer) đều nằm trong lớp thật, nên một costmap giả
 * chỉ mô phỏng lại chúng theo cách người viết test *nghĩ* chúng hoạt động.
 *
 * @note `Layer::Layer()` đặt `enabled_ = false`; layer thật bật cờ này khi đọc config. Ở đây phải tự
 *       bật — và chính chỗ đó cho phép test nhánh "bỏ qua layer đang tắt".
 */
class SpyLayer : public robot_costmap_2d::Layer
{
public:
  struct Record
  {
    const std::type_info* type = nullptr;  ///< type_info có storage tĩnh nên giữ con trỏ là an toàn.
    std::string topic;
  };

  /// @brief Hook để test tự sao chép phần dữ liệu nó quan tâm — con trỏ `data` treo sau khi trả về.
  using Observer = std::function<void(const void*, const std::type_info&, const std::string&)>;

  explicit SpyLayer(robot_costmap_2d::LayerType type, bool enabled = true, bool explode = false)
    : type_(type), explode_(explode)
  {
    enabled_ = enabled;
  }

  robot_costmap_2d::LayerType getType() const override
  {
    return type_;
  }

  void setObserver(Observer observer)
  {
    observer_ = std::move(observer);
  }

  const std::vector<Record>& records() const
  {
    return records_;
  }

  std::size_t count() const
  {
    return records_.size();
  }

protected:
  void handleImpl(const void* data, const std::type_info& type, const std::string& topic) override
  {
    if (explode_)
    {
      throw std::runtime_error("SpyLayer được yêu cầu ném exception");
    }
    records_.push_back(Record{ &type, topic });
    if (observer_)
    {
      observer_(data, type, topic);
    }
  }

private:
  robot_costmap_2d::LayerType type_;
  bool explode_;
  Observer observer_;
  std::vector<Record> records_;
};

using SpyPtr = boost::shared_ptr<SpyLayer>;

/// @brief Cắm một layer gián điệp vào @p costmap và trả con trỏ để kiểm tra sau.
inline SpyPtr attachSpy(robot_costmap_2d::LayeredCostmap& costmap,
                        robot_costmap_2d::LayerType type, const std::string& name,
                        bool enabled = true, bool explode = false)
{
  SpyPtr spy = boost::make_shared<SpyLayer>(type, enabled, explode);
  spy->initialize(&costmap, name, nullptr);
  costmap.addPlugin(spy);
  return spy;
}

}  // namespace testing
}  // namespace move_base2

#endif  // MOVE_BASE2_TEST_SPY_LAYER_H_