From 14208a9ac836f76f5e24fe3ba69de29c8c522bef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ond=C5=99ej=20Svoboda?= <xsvobo25@fi.muni.cz>
Date: Tue, 5 Sep 2023 17:38:52 +0200
Subject: [PATCH] voxel: Reconstruct path from init consuming parent_map

---
 .../voxelReconfig/src/algs/astar/mod.rs       |  4 +--
 .../voxelReconfig/src/algs/astar/opt.rs       |  4 +--
 .../voxelReconfig/src/algs/bfs.rs             |  2 +-
 .../voxelReconfig/src/algs/mod.rs             | 30 +++++++++++++++++++
 4 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/softwareComponents/voxelReconfig/src/algs/astar/mod.rs b/softwareComponents/voxelReconfig/src/algs/astar/mod.rs
index 2385f05c2..3fcd4d460 100644
--- a/softwareComponents/voxelReconfig/src/algs/astar/mod.rs
+++ b/softwareComponents/voxelReconfig/src/algs/astar/mod.rs
@@ -40,9 +40,7 @@ where
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, |parent_info| {
-        parent_info.0.clone()
-    }))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p.0))
 }
 
 fn compute_parents<TGraph, TMetric>(
diff --git a/softwareComponents/voxelReconfig/src/algs/astar/opt.rs b/softwareComponents/voxelReconfig/src/algs/astar/opt.rs
index 2046d7806..635970107 100644
--- a/softwareComponents/voxelReconfig/src/algs/astar/opt.rs
+++ b/softwareComponents/voxelReconfig/src/algs/astar/opt.rs
@@ -42,9 +42,7 @@ where
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, |parent_info| {
-        parent_info.0.clone()
-    }))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p.0))
 }
 
 fn compute_parents<TGraph, TMetric>(
diff --git a/softwareComponents/voxelReconfig/src/algs/bfs.rs b/softwareComponents/voxelReconfig/src/algs/bfs.rs
index 983dc3d73..cbe885dba 100644
--- a/softwareComponents/voxelReconfig/src/algs/bfs.rs
+++ b/softwareComponents/voxelReconfig/src/algs/bfs.rs
@@ -27,7 +27,7 @@ pub fn compute_path<TGraph: StateGraph>(
         .0
         .clone();
 
-    Ok(reconstruct_path_to(goal, &parent_map, Clone::clone))
+    Ok(reconstruct_path_to(goal, parent_map, |p| p))
 }
 
 fn compute_parents<TGraph: StateGraph>(
diff --git a/softwareComponents/voxelReconfig/src/algs/mod.rs b/softwareComponents/voxelReconfig/src/algs/mod.rs
index b0b85efe1..4cdf0d639 100644
--- a/softwareComponents/voxelReconfig/src/algs/mod.rs
+++ b/softwareComponents/voxelReconfig/src/algs/mod.rs
@@ -31,6 +31,36 @@ pub trait StateGraph {
 }
 
 fn reconstruct_path_to<TItem: Eq + std::hash::Hash, TParentInfo, S: BuildHasher>(
+    goal: TItem,
+    mut parent_map: HashMap<TItem, TParentInfo, S>,
+    mut get_parent: impl FnMut(TParentInfo) -> Option<TItem>,
+) -> Vec<TItem> {
+    assert!(parent_map.contains_key(&goal), "Missing goal parent");
+    let mut path = Vec::new();
+    let mut parent = goal;
+    loop {
+        if let Some(new_parent_info) = parent_map.remove(&parent) {
+            if let Some(new_parent) = get_parent(new_parent_info) {
+                path.push(std::mem::replace(&mut parent, new_parent));
+            } else {
+                path.push(parent);
+                break;
+            }
+        } else {
+            if path.contains(&parent) {
+                panic!("Cyclic dependency in parent graph");
+            } else {
+                panic!("Missing parent info");
+            }
+        }
+    }
+
+    path.reverse();
+    path
+}
+
+#[allow(unused)]
+fn reconstruct_path_to_noconsume<TItem: Eq + std::hash::Hash, TParentInfo, S: BuildHasher>(
     goal: TItem,
     parent_map: &HashMap<TItem, TParentInfo, S>,
     mut get_parent: impl FnMut(&TParentInfo) -> Option<TItem>,