From 3907f9b7dd425b3886a9b156d157c324cbf6196d Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Tue, 17 Oct 2023 01:17:23 +0200
Subject: a

---
 src/path-search-dijkstra.cpp | 21 +++++++++++++++++----
 src/path-search-result.cpp   |  4 ++++
 src/path-search-result.hpp   |  3 +++
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 79f94e9d..b5f5355a 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -135,9 +135,6 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
     if (!path_search::is_passable(w, to, to_offset, own_size, own_id, p))
         return {};
 
-    path_search_result result;
-    auto& path = result.path(); path.clear();
-
     cache.allocate(from_, max_dist);
     nodes.push_back({.dist = 0, .pt = from_, });
     add_to_heap(0);
@@ -319,7 +316,23 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
     }
 #endif
 
-    // todo...
+    path_search_result result;
+    auto& path = result.path();
+    path.clear();
+
+    if (auto i = goal_idx; i != (uint32_t)-1)
+    {
+        do {
+            const auto& node = nodes[i];
+            path.push_back(node.pt);
+            i = node.prev;
+
+        } while(i !=(uint32_t)-1);
+
+        result.set_cost(nodes[goal_idx].dist);
+        std::reverse(path.begin(), path.end());
+    }
+
     return result;
 }
 
diff --git a/src/path-search-result.cpp b/src/path-search-result.cpp
index 995ad710..202a8457 100644
--- a/src/path-search-result.cpp
+++ b/src/path-search-result.cpp
@@ -42,6 +42,7 @@ path_search_result::path_search_result(const path_search_result& x) noexcept
     auto self = path_search_result{};
     self._node->vec = x._node->vec;
     _node = std::move(self._node);
+    _cost = x._cost;
 }
 
 path_search_result& path_search_result::operator=(const path_search_result& x) noexcept
@@ -50,11 +51,14 @@ path_search_result& path_search_result::operator=(const path_search_result& x) n
     fm_debug_assert(!_node->_next);
     if (&x != this)
         _node->vec = x._node->vec;
+    _cost = x._cost;
     return *this;
 }
 
 path_search_result::node::node() noexcept = default;
 size_t path_search_result::size() const { return _node->vec.size(); }
+uint32_t path_search_result::cost() const { return _cost; }
+void path_search_result::set_cost(uint32_t value) { _cost = value; }
 auto path_search_result::data() const -> const point* { return _node->vec.data(); }
 path_search_result::operator bool() const { return !_node->vec.empty(); }
 
diff --git a/src/path-search-result.hpp b/src/path-search-result.hpp
index ee3d017d..e6e2d7fd 100644
--- a/src/path-search-result.hpp
+++ b/src/path-search-result.hpp
@@ -15,6 +15,8 @@ struct path_search_result final
     const point* data() const;
     const point& operator[](size_t index) const;
     size_t size() const;
+    uint32_t cost() const;
+    void set_cost(uint32_t value);
 
     std::vector<point>& path();
     const std::vector<point>& path() const;
@@ -48,6 +50,7 @@ private:
     static std::unique_ptr<node> _pool; // NOLINT(*-avoid-non-const-global-variables)
 
     std::unique_ptr<node> _node;
+    uint32_t _cost = 0;
 };
 
 } // namespace floormat
-- 
cgit v1.2.3