From dc232b8fba8ddfbb9a33fcb5185716de425a960f Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Sun, 26 Jun 2022 12:27:38 +0300
Subject: [PATCH] Support edge visit callbacks in util::pathfinder

---
 libs/util/include/psemek/util/find_path.hpp | 24 ++++++++++++++++++---
 1 file changed, 21 insertions(+), 3 deletions(-)

diff --git a/libs/util/include/psemek/util/find_path.hpp b/libs/util/include/psemek/util/find_path.hpp
index 8c1dcbe5..5236c42f 100644
--- a/libs/util/include/psemek/util/find_path.hpp
+++ b/libs/util/include/psemek/util/find_path.hpp
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <psemek/util/functional.hpp>
+
 #include <unordered_map>
 #include <set>
 #include <deque>
@@ -8,7 +10,7 @@
 namespace psemek::util
 {
 
-	template <typename Cost, typename Node, typename NeighboursFn, typename HeuristicFn>
+	template <typename Cost, typename Node, typename NeighboursFn, typename HeuristicFn, typename EdgeCallback>
 	struct pathfinder
 	{
 		struct node_compare
@@ -30,15 +32,17 @@ namespace psemek::util
 
 		NeighboursFn node_neighbours;
 		HeuristicFn heuristic;
+		EdgeCallback edge_callback;
 
 		std::unordered_map<Node, Cost> cost;
 		std::unordered_map<Node, Cost> priority;
 		std::unordered_map<Node, Node> previous;
 		std::set<Node, node_compare> queue;
 
-		pathfinder(NeighboursFn && node_neighbours, HeuristicFn && heuristic)
+		pathfinder(NeighboursFn && node_neighbours, HeuristicFn && heuristic, EdgeCallback && edge_callback)
 			: node_neighbours(std::forward<NeighboursFn>(node_neighbours))
 			, heuristic(std::forward<HeuristicFn>(heuristic))
+			, edge_callback(std::forward<EdgeCallback>(edge_callback))
 			, queue(node_compare{priority})
 		{}
 
@@ -72,6 +76,8 @@ namespace psemek::util
 			node_neighbours(node, [&](Node const & neighbour, Cost const & edge_cost){
 				Cost const new_cost = node_cost + edge_cost;
 
+				edge_callback(node, node_cost, neighbour, edge_cost);
+
 				auto it = cost.find(neighbour);
 
 				if (it == cost.end() || new_cost < it->second)
@@ -117,10 +123,22 @@ namespace psemek::util
 		}
 	};
 
+	template <typename Cost, typename Node, typename NeighboursFn, typename HeuristicFn, typename EdgeCallback>
+	auto make_pathfinder(NeighboursFn && node_neighbours, HeuristicFn && heuristic, EdgeCallback && edge_callback)
+	{
+		return pathfinder<Cost, Node, NeighboursFn, HeuristicFn, EdgeCallback>(std::forward<NeighboursFn>(node_neighbours), std::forward<HeuristicFn>(heuristic), std::forward<EdgeCallback>(edge_callback));
+	}
+
+	template <typename Cost, typename Node, typename NeighboursFn, typename HeuristicFn>
+	auto make_pathfinder(NeighboursFn && node_neighbours, HeuristicFn && heuristic)
+	{
+		return pathfinder<Cost, Node, NeighboursFn, HeuristicFn, decltype(util::nop)>(std::forward<NeighboursFn>(node_neighbours), std::forward<HeuristicFn>(heuristic), util::nop);
+	}
+
 	template <typename Cost, typename Node, typename NeighboursFn, typename HeuristicFn>
 	std::optional<std::deque<Node>> find_path(Node const & start, Node const & end, NeighboursFn && node_neighbours, HeuristicFn && heuristic)
 	{
-		pathfinder<Cost, Node, NeighboursFn, HeuristicFn> helper(std::forward<NeighboursFn>(node_neighbours), std::forward<HeuristicFn>(heuristic));
+		auto helper = make_pathfinder<Cost, Node>(std::forward<NeighboursFn>(node_neighbours), std::forward<HeuristicFn>(heuristic));
 
 		helper.init(start);
 		bool found = false;