const vector<vertex<string, int>> vertices =
{
    vertex<string, int>("A", {{ "B", 1 }, { "C", 4 },{ "F", 2 }}),
    vertex<string, int>("B", {{ "E", 2 }}),
    vertex<string, int>("C", {{ "G", 2 }, { "D", 4 }}),
    vertex<string, int>("D", {}),
    vertex<string, int>("E", {{ "D", 3 }}),
    vertex<string, int>("F", {{ "C", 1 }, { "G", 4 }}),
    vertex<string, int>("G", {{ "E", 5 }}),
};

#include "stdafx.h"

    #include <unordered_map>
    #include <vector>
    #include <limits>
    #include <algorithm>
    #include <iostream>
    #include <map>

    using namespace std;

    ostream& operator<<(ostream& os, const string &a)
    {
        os << a.c_str();
        return os;
    }

    template <class T, class T2>
    class vertex
    {
    public:
        T name;
        unordered_map<T, T2> edges;

        vertex(T n, const unordered_map<T, T2>& e)
        {
            name = n;
            edges = e;
        }
    };

    template <class T, class T2>
    class graph
    {
        unordered_map<T, const unordered_map<T, T2>> vertices_;

    public:
        graph() {}

        explicit graph(const vector<vertex<T, T2>> &vertices)
        {
            for (auto vertex : vertices)
            {
                add_vertex(vertex.name, vertex.edges);
            }
        }

        void operator +=(vertex<T, T2> &v)
        {
            add_vertex(v.name, v.edges);
        }

        void add_vertex(T name, const unordered_map<T, T2>& edges)
        {
            // Insert the connected nodes in unordered map
            vertices_.insert(unordered_map<T, const unordered_map<T, T2>>::value_type(name, edges));
        }

        vector<T> shortest_path(T start, T finish)
        {
            // Second arguments -> distances
            // Find the smallest distance in the already in closed list and push it in -> previous
            unordered_map<T, T2> distances;
            unordered_map<T, T> previous;

            vector<T> nodes;    // Open list
            vector<T> path;     // Closed list

            const auto comparator2 = [&](T left, T right) { return distances[left] > distances[right]; };

            // initialize distances and nodes
            for (auto& vertex : vertices_)
            {
                distances[vertex.first] = (vertex.first == start) ? 0 : numeric_limits<T2>::max();
                nodes.push_back(vertex.first);
            }
            sort(nodes.begin(), nodes.end(), comparator2);

            // while nodes is not empty
            while (!nodes.empty())
            {
                // the smallest is last
                // pop it off the list
                auto smallest = nodes.back();
                nodes.pop_back();

                // see if we have our target
                if (smallest == finish)
                {
                    // unwind to get the path
                    while (previous.find(smallest) != end(previous))
                    {
                        path.push_back(smallest);
                        smallest = previous[smallest];
                    }
                    // exit we are done!
                    break;
                }

                // if the smallest node has not been visited the exit
                if (distances[smallest] == numeric_limits<T2>::max())
                {
                    break;
                }

                // visit neighbors
                auto needsort = false;
                for (auto& neighbor : vertices_[smallest])
                {
                    // neighbor.first is the neighbors name
                    // neighbor.second is the neighbors distance
                    const auto alt = distances[smallest] + neighbor.second;
                    if (alt < distances[neighbor.first])
                    {
                        distances[neighbor.first] = alt;
                        previous[neighbor.first] = smallest;
                        needsort = true;
                    }
                }
                // re sort
                if (needsort)
                    sort(nodes.begin(), nodes.end(), comparator2);
            }

            cout << endl << "Shortest path:" << endl;
            for (auto a : path)
            {
                cout << " " << a << " distance: " << distances[a] << endl;
            }
            cout << " " << start << " distance: " << distances[start] << endl;
            return path;
        }
    };

    int main()
    {
        const vector<vertex<string, int>> vertices =
        {
            vertex<string, int>("A", {{ "B", 1 }, { "C", 4 },{ "F", 2 }}),
            vertex<string, int>("B", {{ "E", 2 }}),
            vertex<string, int>("C", {{ "G", 2 }, { "D", 4 }}),
            vertex<string, int>("D", {}),
            vertex<string, int>("E", {{ "D", 3 }}),
            vertex<string, int>("F", {{ "C", 1 }, { "G", 4 }}),
            vertex<string, int>("G", {{ "E", 5 }}),
        };

        graph<string, int> g(vertices);

        const string initNode = "A";
        const string destNode = "G";

        cout << "Initial node: " << initNode << endl;
        cout << "Goal node: " << destNode << endl;

        auto result = g.shortest_path(initNode, destNode);

        return 0;
    }