From 2ffb781958f85c82fc28c9efbeeb23f91e844a9c Mon Sep 17 00:00:00 2001
From: Warren Ulrich <warren.ulrich@outlook.com>
Date: Sat, 10 Jun 2023 03:48:21 -0700
Subject: [PATCH] Initial commit

---
 .gitignore                          |   2 +
 CMakeLists.txt                      |  10 ++
 include/pathfinding/pathfinding.hpp | 144 ++++++++++++++++++++++++++++
 3 files changed, 156 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 CMakeLists.txt
 create mode 100644 include/pathfinding/pathfinding.hpp

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..01f9cb9
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+build/
+.vscode/
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..b6c3ce0
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.26)
+
+project(pathfinding)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+add_library(${PROJECT_NAME} INTERFACE)
+
+target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include)
\ No newline at end of file
diff --git a/include/pathfinding/pathfinding.hpp b/include/pathfinding/pathfinding.hpp
new file mode 100644
index 0000000..af94df9
--- /dev/null
+++ b/include/pathfinding/pathfinding.hpp
@@ -0,0 +1,144 @@
+#pragma once
+
+#include <filesystem>
+#include <unordered_set>
+#include <fstream>
+#include <queue>
+#include <set>
+#include <variant>
+
+#include <Game/Core.hpp>
+
+namespace std {
+    template <>
+    struct hash<Tile> {
+       std::size_t operator()(const Tile& tile) const {
+            std::size_t h1 = std::hash<std::int32_t>()(tile.X);
+            std::size_t h2 = std::hash<std::int32_t>()(tile.Y);
+            std::size_t h3 = std::hash<std::int32_t>()(tile.Plane);
+            return h1 ^ (h2 << 1) ^ (h3 << 2);
+        }
+    };
+}
+
+namespace pathfinding {
+    std::unordered_set<std::int32_t> mapped_regions;
+    std::unordered_map<Tile, Pathfinding::COLLISION_FLAG> collision_data;
+
+    void load_collision_data(std::ifstream& file) {
+        std::string line;
+        std::getline(file, line); // skip csv header
+        while (std::getline(file, line)) {
+            std::stringstream ss(line);
+            std::string token;
+            std::vector<std::string> tokens;
+            while (std::getline(ss, token, ',')) {
+                tokens.emplace_back(token);
+            }
+
+            const auto region = std::stoi(tokens[0]);
+            const auto plane = std::stoi(tokens[1]);
+            const auto x = std::stoi(tokens[2]);
+            const auto y = std::stoi(tokens[3]);
+            const auto flag = static_cast<Pathfinding::COLLISION_FLAG>(std::stoi(tokens[4]));
+
+            mapped_regions.insert(region);
+            collision_data.emplace(Tile(x, y, plane), flag);
+        }
+    }
+
+    void visit_neighbors(const Tile &tile, const auto &func) {
+        Pathfinding::COLLISION_FLAG flags = Pathfinding::COLLISION_FLAG::OPEN;
+        auto it = collision_data.find(tile);
+        if (it != collision_data.end()) {
+            flags = it->second;
+        }
+
+        const auto blocked = [](Pathfinding::COLLISION_FLAG flags) {
+            return (flags & Pathfinding::BLOCKED) || (flags & Pathfinding::OCCUPIED);
+        };
+
+        constexpr std::array<std::tuple<int, int, Pathfinding::COLLISION_FLAG>, 4> directions = {{
+            {0, 1, Pathfinding::COLLISION_FLAG::NORTH},
+            {1, 0, Pathfinding::COLLISION_FLAG::EAST},
+            {0, -1, Pathfinding::COLLISION_FLAG::SOUTH},
+            {-1, 0, Pathfinding::COLLISION_FLAG::WEST}
+        }};
+
+        for (const auto &[dx, dy, dirFlag] : directions) {
+            if ((flags & dirFlag) == Pathfinding::COLLISION_FLAG::OPEN) {
+                int newX = tile.X + dx;
+                int newY = tile.Y + dy;
+
+                auto neighbor = Tile(newX, newY, tile.Plane);
+
+                auto neighborIt = collision_data.find(neighbor);
+                if (neighborIt != collision_data.end()) {
+                    if (!blocked(neighborIt->second)) {
+                        func(neighbor);
+                    }
+                } else {
+                    func(neighbor);
+                }
+            }
+        }
+    }
+
+    using path_step = std::variant<Tile, obstacle*>;
+    
+    std::vector<Tile> find_path(const Tile &start, const Tile &end) {
+        class path_node {
+        public:
+            Tile tile;
+            std::shared_ptr<path_node> parent;
+            double cost;
+
+            path_node(const Tile &tile, std::shared_ptr<path_node> parent, double cost) 
+                : tile(tile), parent(parent), cost(cost) {}
+        };
+
+        const auto compare = [](const std::shared_ptr<path_node> &a, const std::shared_ptr<path_node> &b) {
+            return a->cost > b->cost;
+        };
+
+        const auto heuristic = [](const Tile& a, const Tile& b) {
+            return std::abs(a.X - b.X) + std::abs(a.Y - b.Y);
+        };
+
+        std::priority_queue<std::shared_ptr<path_node>, std::vector<std::shared_ptr<path_node>>, decltype(compare)> queue(compare);
+
+        std::unordered_map<Tile, double, std::hash<Tile>> costs;
+        std::unordered_set<Tile, std::hash<Tile>> visited;
+
+        queue.emplace(std::make_shared<path_node>(start, nullptr, 0.0));
+        costs[start] = 0.0;
+
+        while (!queue.empty()) {
+            auto current = queue.top();
+            queue.pop();
+
+            if (current->tile == end) {
+                std::vector<Tile> path;
+                while (current != nullptr) {
+                    path.push_back(current->tile);
+                    current = current->parent;
+                }
+                std::reverse(path.begin(), path.end());
+                return path;
+            }
+
+            visit_neighbors(current->tile, [&](const Tile &neighbor) {
+                double new_cost = costs[current->tile] + 1; // Assuming constant cost of 1 for each step
+
+                if (visited.find(neighbor) == visited.end() || new_cost < costs[neighbor]) {
+                    costs[neighbor] = new_cost;
+                    double priority = new_cost + heuristic(neighbor, end);
+                    queue.emplace(std::make_shared<path_node>(neighbor, current, priority));
+                    visited.insert(neighbor);
+                }
+            });
+        }
+
+        return std::vector<Tile>(); // no path found
+    }
+}
\ No newline at end of file