navigation/include/pathfinding/pathfinding.hpp

#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);
                }
            }
        }
    }
    
    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
    }
}