#include <vector>
#include <thread>
#include <cassert>
#include <functional>
#include <iostream>
#include <algorithm>
#include <optional>
#include <future>
#include <chrono>
#include <fstream>

namespace graph
{
    template <class TVertexDescriptor, class TNode, class TNodeCompare = std::less<TNode>, class TKeyHash = std::hash<TVertexDescriptor>, class TKeyEqual = std::equal_to<TVertexDescriptor>>
    class NodeMap
    {
    public:
        explicit NodeMap(TNodeCompare _nodeCompare = TNodeCompare(), TKeyHash _hash = TKeyHash(), TKeyEqual _equal = TKeyEqual()) :
            m_Nodes(10, std::move(_hash), std::move(_equal)),   // ToDo: not sure if that bucket_count parameter fits well; possible other solution available?
            m_NodeCompare(std::move(_nodeCompare))
        {
        }
        void insert(TVertexDescriptor _key, TNode _node)
        {
            if (auto result = m_Nodes.insert({ _key, _node });
                !result.second && m_NodeCompare(_node, result.first->second))
            {
                result.first->second = _node;
            }
        }

        auto take_less()
        {
            auto itr = std::min_element(std::begin(m_Nodes), std::end(m_Nodes),
                [&comp = m_NodeCompare](const auto& _lhs, const auto& _rhs) { return comp(_lhs.second, _rhs.second); }
            );
            assert(itr != std::end(m_Nodes));
            auto node = std::move(*itr);
            m_Nodes.erase(itr);
            return node;
        }

        const TNode* find(const TVertexDescriptor& _key) const
        {
            auto itr = m_Nodes.find(_key);
            return std::end(m_Nodes) == itr ? nullptr : &itr->second;
        }

        bool contains(const TVertexDescriptor& _key) const
        {
            return m_Nodes.count(_key) == 1;    // ToDo: use std::unordered_map::contains in C++20
        }

        bool empty() const
        {
            return std::empty(m_Nodes);
        }

    private:
        std::unordered_map<TVertexDescriptor, TNode, TKeyHash, TKeyEqual> m_Nodes;
        TNodeCompare m_NodeCompare;
    };

    template <class TVertexDescriptor, class TNeighbourFinder>
    class BreadthFirstSearch
    {
    public:
        template <class TTNeighbourFinder>
        explicit BreadthFirstSearch(TVertexDescriptor _start, TTNeighbourFinder&& _neighbourFinder) :
            m_NeighbourFinder(std::forward<TTNeighbourFinder>(_neighbourFinder))
        {
            m_OpenList.insert(std::move(_start), Node());
        }

        std::optional<TVertexDescriptor> next()
        {
            if (std::empty(m_OpenList))
                return std::nullopt;
            auto node = m_OpenList.take_less();
            m_ClosedList.insert(node.first, node.second);
            m_NeighbourFinder(node.first,
                [&openList = m_OpenList, &closedList = m_ClosedList, node = Node{ node.first, node.second.cost + 1 }](const auto& _vertex)
                {
                    if (!closedList.contains(_vertex))
                    {
                        openList.insert(_vertex, node);
                    }
                }
            );
            return node.first;
        }

        std::optional<std::vector<TVertexDescriptor>> extract_path(const TVertexDescriptor& _vertex) const
        {
            if (auto curNode = m_ClosedList.find(_vertex))
            {
                std::vector<TVertexDescriptor> path{ _vertex };
                while (curNode && curNode->parent)
                {
                    path.emplace_back(*curNode->parent);
                    curNode = m_ClosedList.find(*curNode->parent);
                }
                std::reverse(std::begin(path), std::end(path));
                return path;
            }
            return std::nullopt;
        }

    private:
        struct Node
        {
            std::optional<TVertexDescriptor> parent;
            int cost = 0;

            friend bool operator <(const Node& _lhs, const Node& _rhs)
            {
                return _lhs.cost < _rhs.cost;
            }
        };

        NodeMap<TVertexDescriptor, Node> m_OpenList;
        NodeMap<TVertexDescriptor, Node> m_ClosedList;
        TNeighbourFinder m_NeighbourFinder;
    };

    template <class TVertexDescriptor, class TNeighbourFinder>
    auto make_BreadthFirstSearch(TVertexDescriptor _start, TNeighbourFinder&& _neighbourFinder)
    {
        return BreadthFirstSearch<TVertexDescriptor, TNeighbourFinder>(std::move(_start), std::forward<TNeighbourFinder>(_neighbourFinder));
    }

    template <class TVertexDescriptor, class TCallable, typename = std::enable_if_t<std::is_invocable_r_v<bool, TCallable, const TVertexDescriptor&>>>
    bool _reached_destination(const TVertexDescriptor& _current, TCallable&& _func)
    {
        return _func(_current);
    }

    template <class TVertexDescriptor>
    bool _reached_destination(const TVertexDescriptor& _current, const TVertexDescriptor& _destination)
    {
        return _current == _destination;
    }

    template <class TGoal, class TPathFinder>
    auto find_path(TGoal&& _destination, TPathFinder&& _pathFinder) -> std::optional<std::vector<int>>
    {
        while (auto current = _pathFinder.next())
        {
            if (_reached_destination(*current, _destination))
            {
                return _pathFinder.extract_path(*current);
            }
        }
        return std::nullopt;
    }
} // namespace graph

template <class TDestination, class TNeighbourFinder>
void search_path_and_print(int _from, TDestination&& _to, TNeighbourFinder&& _neighbourFinder)
{
    for (auto node : graph::find_path(std::forward<TDestination>(_to), graph::make_BreadthFirstSearch(_from, std::forward<TNeighbourFinder>(_neighbourFinder))).value_or(std::vector<int>()))
    {
        std::cout << node << std::endl;
    }
}

int main()
{
    std::vector<int> nodes{ 1, 2, 3, 4, 5, 6, 7 };
    std::vector<std::pair<int, int>> connections
    {
        { 1, 2 },
        { 2, 3 },
        { 2, 4 },
        { 4, 5 },
        { 3, 6 },
        { 3, 7 },
        { 6, 7 },
        { 7, 1 }
    };

    auto neighbourFinder = [&connections](const auto& _vertex, auto&& _callback)
    {
        auto next = [itr = std::begin(connections), end = std::end(connections)](const auto& _vertex) mutable->std::optional<int>
        {
            itr = std::find_if(itr, end, [&_vertex](const auto& _pair) { return _pair.first == _vertex || _pair.second == _vertex; });
            if (itr != end)
            {
                auto result = itr->first == _vertex ? itr->second : itr->first;
                ++itr;
                return result;
            }
            return std::nullopt;
        };

        while (auto neigh = next(_vertex))
            _callback(*neigh);
    };

    search_path_and_print(1, 6, neighbourFinder);
    search_path_and_print(1, [](const auto& _node) { return _node == 6; }, neighbourFinder);
    std::cin.get();
}