#include <iostream>
#include <queue>
#include <tuple>
#include <vector>
#include <unordered_set>
#include <unordered_map>
#include <type_traits>
#include <limits>

using std::pair;
using std::string;
using std::unordered_map;
using std::unordered_set;
using std::vector;
using std::cout;
using std::endl;
using std::priority_queue;

template  <typename T, typename W,
        typename std::enable_if<std::is_arithmetic<W>::value, W>::type* = nullptr>
struct QueueElem {

    QueueElem(const T& element, const W& weight)
      : element(element), weight(weight)
      {
      }

    QueueElem() = default;
    ~QueueElem() = default;
    QueueElem(const QueueElem& other) = default;
    QueueElem& operator=(const QueueElem& other) = default;
    QueueElem(QueueElem&& other) = default;
    QueueElem& operator=(QueueElem&& other) = default;

    T element;
    W weight;

    bool operator<(const QueueElem& other) const {
        // switch signs because we need a min-priority queue
        return weight > other.weight;
    }
};

template  <typename T, typename W,
           typename std::enable_if<std::is_arithmetic<W>::value, W>::type* = nullptr>
class Graph {

public:

    void insertEdge(const T& from, const T& to, const W& weight = 0.0) {

        insertNode(from);
        insertNode(to);
        auto found = adj.find(from);
        if (found != adj.end()) {
            found->second.emplace(to, weight);
        }
    }

    std::pair<W, std::vector<T>> dijkstra(const T& from, const T& to) {

        unordered_map<T, W> distances;
        unordered_map<T, T> previous;

        for (const auto& kv : adj) {
            distances.emplace(kv.first, std::numeric_limits<W>::max());
        }
        distances[from] = 0;

        priority_queue<QueueElem<T, W>> pq;
        pq.emplace(QueueElem<T, W>(from, 0));
        while (!pq.empty()) {
            auto current = pq.top();
            if (current.element == to) {
                std::vector<T> path;

                T current = to;
                while (current != from) {
                    path.insert(path.begin(), current);
                    current = previous[current];
                }
                path.insert(path.begin(), from);

                return {distances[to], path};
            }
            pq.pop();
            for (const auto& kv : adj[current.element]) {
                auto dst = kv.first;
                auto cost = kv.second;
                auto estimate = current.weight + adj[current.element][dst];
                auto curr_dist = distances[kv.first];
                if (estimate < curr_dist) {
                    distances[dst] = estimate;
                    pq.emplace(QueueElem<T, W>(dst, estimate));
                    previous[dst] = current.element;
                }
            }

        }
    }

    void printGraph() const {
        std::cout << "\n[\n";
        for (const auto& kv : adj) {
          std::cout << "  " << kv.first << " -> [ ";
          for (const auto& kv2 : kv.second) {
            std::cout << "(" << kv2.first << ", " << kv2.second << ") ";
          }
          std::cout << "]\n";
        }
        std::cout << "]\n" << std::endl;
    }


private:
    std::unordered_map<T, unordered_map<T, W>> adj;

    void insertNode(const T& node) {
        if (adj.find(node) == adj.end()) {
            adj.emplace(node, unordered_map<string, W>());
        }
    }



};

int main() {

    Graph<string, int> g;
    g.insertEdge("Ajo", "Bordo", 50);
    g.insertEdge("Ajo", "Danza", 80);
    g.insertEdge("Bordo", "Danza", 90);
    g.insertEdge("Bordo", "Colina", 60);
    g.insertEdge("Danza", "Erizo", 70);
    g.insertEdge("Colina", "Erizo", 40);
    g.insertEdge("Danza", "Colina", 20);
    g.insertEdge("Erizo", "Bordo", 50);

    cout << "Graph";
    g.printGraph();

    cout << "Ajo -> Erizo: " << endl;
    cout << "Expected distance: 140" << endl;
    cout << "Epected path: Ajo -> Danza -> Colina -> Erizo" << endl;
    auto [shortest, path] = g.dijkstra("Ajo", "Erizo");
    cout << "\n\nResults:\n\n";
    cout << "shortest distance: " << shortest << endl;
    cout << "path: ";
    for (const auto& elem : path) {
        cout << elem << " ";
    }
    cout << endl;

    return 0;
}