Feature/dynamic node

include/warthog/memory/node_pool.h

@@ -24,26 +24,109 @@
 //
 
 #include "cpool.h"
+#include <concepts>
+#include <cstdint>
+#include <memory>
+#include <tuple>
 #include <warthog/search/search_node.h>
 
-#include <stdint.h>
-
 namespace warthog::memory
 {
 
 namespace node_pool_ns
 {
-static const uint64_t NBS      = 8; // node block size; set this >= 8
-static const uint64_t LOG2_NBS = 3;
-static const uint64_t NBS_MASK = 7;
+constexpr uint64_t LOG2_NBS = 6; // node block size = 2^n, n >= 3
+constexpr uint64_t NBS      = 1 << LOG2_NBS;
+constexpr uint64_t NBS_MASK = NBS - 1;
+static_assert(LOG2_NBS >= 3, "must be at least 3 for size of 8");
 }
 
 class node_pool
 {
 public:
+	struct data_deleter_ptr
+	{
+		void (*del)(void*) = nullptr;
+
+		constexpr data_deleter_ptr() noexcept = default;
+		constexpr data_deleter_ptr(void (*p)(void*)) noexcept : del(p) { }
+
+		void
+		operator()(void* data) const noexcept
+		{
+			(*del)(data);
+		}
+	};
+	node_pool();
 	node_pool(size_t num_nodes);
 	~node_pool();
 
+	template<
+	    std::derived_from<search::search_node> T = search::search_node,
+	    typename... NodeArgs>
+	void
+	set_type(NodeArgs... node_args) noexcept
+	{
+		clear();
+		using arg_type = std::tuple<NodeArgs...>;
+		if constexpr(sizeof...(NodeArgs) != 0)
+		{
+			create_block_data_ = static_cast<void*>(
+			    new arg_type(std::forward<NodeArgs&&>(node_args)...));
+		}
+		block_type_sizes_ = sizeof(T);
+		size_t block_sz   = node_pool_ns::NBS * sizeof(T);
+		blockspool_       = std::make_unique<cpool>(block_sz, 1);
+		create_block_     = [](void* block_p, sn_id_t block_id,
+                           void* data) noexcept {
+            assert(block_p != nullptr);
+            T* block          = reinterpret_cast<T*>(block_p);
+            pad_id current_id = pad_id{block_id << node_pool_ns::LOG2_NBS};
+            for(uint32_t i = 0; i < node_pool_ns::NBS; ++i, ++current_id.id)
+            {
+                if constexpr(sizeof...(NodeArgs) == 0)
+                {
+                    std::construct_at(block + i, current_id);
+                }
+                else
+                {
+                    assert(data != nullptr);
+                    std::apply(
+                        [block_i = block + i,
+                         current_id]<typename... NodeX>(NodeX&&... args) {
+                            std::construct_at(block_i, current_id, args...);
+                        },
+                        *static_cast<arg_type*>(data));
+                }
+            }
+		};
+		clear_ = [](node_pool& np) noexcept {
+			if(np.blocks_)
+			{
+				for(size_t i = 0; i < np.num_blocks_; ++i)
+				{
+					T* nodes = reinterpret_cast<T*>(np.blocks_[i]);
+					if(nodes != nullptr)
+					{
+						for(uint32_t j = 0; j < node_pool_ns::NBS; ++j)
+						{
+							std::destroy_at(nodes + j);
+						}
+					}
+				}
+			}
+			if constexpr(sizeof...(NodeArgs) == 0)
+			{
+				if(!np.create_block_data_)
+				{
+					arg_type* args
+					    = static_cast<arg_type*>(np.create_block_data_);
+					delete args;
+				}
+			}
+		};
+	}
+
 	// return a warthog::search_node object corresponding to the given id.
 	// if the node has already been generated, return a pointer to the
 	// previous instance; otherwise allocate memory for a new object.
@@ -58,13 +141,24 @@ class node_pool
 	size_t
 	mem();
 
+	// reset nodes
+	void
+	clear();
+
 private:
 	void
 	init(size_t nblocks);
+	void
+	release();
 
-	size_t num_blocks_;
-	search::search_node** blocks_;
-	cpool* blockspool_;
+	size_t num_blocks_ = 0;
+	std::unique_ptr<std::byte*[]> blocks_;
+	size_t block_type_sizes_ = 0;
+	std::unique_ptr<cpool> blockspool_;
+	void (*create_block_)(void* block, sn_id_t bock_id, void* data) noexcept
+	    = nullptr;
+	void (*clear_)(node_pool& np) = nullptr;
+	void* create_block_data_;
 	//        uint64_t* node_init_;
 	//        uint64_t node_init_sz_;
 };

include/warthog/search/search_node.h

@@ -16,23 +16,15 @@
 namespace warthog::search
 {
 
-class search_node
+struct search_node
 {
-public:
-	search_node(pad_id id = pad_id::max())
-	    : id_(id), parent_id_(warthog::SN_ID_MAX), g_(warthog::COST_MAX),
-	      f_(warthog::COST_MAX), ub_(warthog::COST_MAX), status_(0),
-	      priority_(warthog::INF32), search_number_(UINT32_MAX)
-	{
-		refcount_++;
-	}
-
-	~search_node() { refcount_--; }
+	search_node() noexcept = default;
+	search_node(pad_id id = pad_id::max()) noexcept : id_(id) { }
 
 	inline void
 	init(
 	    uint32_t search_number, pad_id parent_id, cost_t g, cost_t f,
-	    cost_t ub = warthog::COST_MAX)
+	    cost_t ub = warthog::COST_MAX) noexcept
 	{
 		parent_id_     = parent_id;
 		f_             = f;
@@ -43,103 +35,103 @@ class search_node
 	}
 
 	inline uint32_t
-	get_search_number() const
+	get_search_number() const noexcept
 	{
 		return search_number_;
 	}
 
 	inline void
-	set_search_number(uint32_t search_number)
+	set_search_number(uint32_t search_number) noexcept
 	{
 		search_number_ = search_number;
 	}
 
 	inline pad_id
-	get_id() const
+	get_id() const noexcept
 	{
 		return id_;
 	}
 
 	inline void
-	set_id(pad_id id)
+	set_id(pad_id id) noexcept
 	{
 		id_ = id;
 	}
 
 	inline bool
-	get_expanded() const
+	get_expanded() const noexcept
 	{
 		return status_;
 	}
 
 	inline void
-	set_expanded(bool expanded)
+	set_expanded(bool expanded) noexcept
 	{
 		status_ = expanded;
 	}
 
 	inline pad_id
-	get_parent() const
+	get_parent() const noexcept
 	{
 		return parent_id_;
 	}
 
 	inline void
-	set_parent(pad_id parent_id)
+	set_parent(pad_id parent_id) noexcept
 	{
 		parent_id_ = parent_id;
 	}
 
 	inline uint32_t
-	get_priority() const
+	get_priority() const noexcept
 	{
 		return priority_;
 	}
 
 	inline void
-	set_priority(uint32_t priority)
+	set_priority(uint32_t priority) noexcept
 	{
 		priority_ = priority;
 	}
 
 	inline cost_t
-	get_g() const
+	get_g() const noexcept
 	{
 		return g_;
 	}
 
 	inline void
-	set_g(cost_t g)
+	set_g(cost_t g) noexcept
 	{
 		g_ = g;
 	}
 
 	inline cost_t
-	get_f() const
+	get_f() const noexcept
 	{
 		return f_;
 	}
 
 	inline void
-	set_f(cost_t f)
+	set_f(cost_t f) noexcept
 	{
 		f_ = f;
 	}
 
 	inline cost_t
-	get_ub() const
+	get_ub() const noexcept
 	{
 		return ub_;
 	}
 
 	inline void
-	set_ub(cost_t ub)
+	set_ub(cost_t ub) noexcept
 	{
 		ub_ = ub;
 	}
 
 	inline void
-	relax(cost_t g, pad_id parent_id)
+	relax(cost_t g, pad_id parent_id) noexcept
 	{
 		assert(g < g_);
 		f_ = (f_ - g_) + g;
@@ -149,7 +141,7 @@ class search_node
 	}
 
 	inline bool
-	operator<(const search_node& other) const
+	operator<(const search_node& other) const noexcept
 	{
 		//    static uint64_t SIGN_MASK = UINT64_MAX & (1ULL<<63);
 		//    cost_t result = this->f_ - other.f_;
@@ -170,7 +162,7 @@ class search_node
 	}
 
 	inline bool
-	operator>(const search_node& other) const
+	operator>(const search_node& other) const noexcept
 	{
 		if(f_ > other.f_) { return true; }
 		if(f_ < other.f_) { return false; }
@@ -181,65 +173,49 @@ class search_node
 	}
 
 	inline bool
-	operator==(const search_node& other) const
+	operator==(const search_node& other) const noexcept
 	{
 		if(!(*this < other) && !(*this > other)) { return true; }
 		return false;
 	}
 
 	inline bool
-	operator<=(const search_node& other) const
+	operator<=(const search_node& other) const noexcept
 	{
 		if(*this < other) { return true; }
 		if(!(*this > other)) { return true; }
 		return false;
 	}
 
 	inline bool
-	operator>=(const search_node& other) const
+	operator>=(const search_node& other) const noexcept
 	{
 		if(*this > other) { return true; }
 		if(!(*this < other)) { return true; }
 		return false;
 	}
 
-	inline void
-	print(std::ostream& out) const
-	{
-		out << "search_node id:" << get_id().id;
-		out << " p_id: ";
-		out << parent_id_.id;
-		out << " g: " << g_ << " f: " << this->get_f() << " ub: " << ub_
-		    << " expanded: " << get_expanded() << " "
-		    << " search_number_: " << search_number_;
-	}
+	void
+	print(std::ostream& out) const;
 
 	uint32_t
-	mem()
+	mem() noexcept
 	{
 		return sizeof(*this);
 	}
 
-	static uint32_t
-	get_refcount()
-	{
-		return refcount_;
-	}
-
-private:
-	pad_id id_;
-	pad_id parent_id_;
+	pad_id id_        = pad_id(warthog::SN_ID_MAX);
+	pad_id parent_id_ = pad_id(warthog::SN_ID_MAX);
 
-	cost_t g_;
-	cost_t f_;
-	cost_t ub_;
+	cost_t g_  = warthog::COST_MAX;
+	cost_t f_  = warthog::COST_MAX;
+	cost_t ub_ = warthog::COST_MAX;
 
 	// TODO steal the high-bit from priority instead of ::status_ ?
-	uint8_t status_;    // open or closed
-	uint32_t priority_; // expansion priority
+	uint8_t status_    = 0;              // open or closed
+	uint32_t priority_ = warthog::INF32; // expansion priority
 
-	uint32_t search_number_;
-	static uint32_t refcount_;
+	uint32_t search_number_ = UINT32_MAX;
 };
 
 struct cmp_less_search_node