#pragma once

#include <vector>
#include <queue>
#include <algorithm>
#include <string.h>
template <size_t N, typename COST_T>
class Dijkstra
{
public:
	struct Path
	{
		int connectingIndex;
		COST_T cost;
	};

	struct Agent
	{
		int index;
		int prev_index;
		COST_T cost;

		bool operator<(const Agent& rhs) const
		{
			return cost > rhs.cost;
		}
	};

	Dijkstra()
	{
		init_paths();
	}

	void init_paths()
	{
		edges.assign(N, std::vector<Path>());
	}

	void add_path(int from, int to, COST_T cost)
	{
		edges[from].push_back(Path{ to, cost });
	}

	void add_path_undirected(int node1, int node2, COST_T cost)
	{
		edges[node1].push_back(Path{ node2, cost });
		edges[node2].push_back(Path{ node1, cost });
	}

	// returns cost
	// returns -1 if there is no reachable path
	COST_T find_shortest_path(int start, int goal)
	{
		// initialize
		que = std::priority_queue<Agent>();
		this->start = start;
		this->goal = goal;
		memset(defined, 0, sizeof defined);

		que.push(Agent{ start });
		while (!que.empty())
		{
			Agent agent = que.top(); que.pop();
				
			if (defined[agent.index])
			{
				continue;
			}
			defined[agent.index] = true;
			prev_node[agent.index] = agent.prev_index;

			if (agent.index == goal)
			{
				return agent.cost;
			}

			for (auto path : edges[agent.index])
			{
				if (!defined[path.connectingIndex])
				{
					que.push(Agent{ path.connectingIndex,
					                agent.index,
					                agent.cost + path.cost });
				}
			}
		}

		return -1;
	}

	std::vector<int> get_shortest_path() const
	{
		std::vector<int> path;
		int agent = goal;
		while (agent != start)
		{
			path.push_back(agent);
			agent = prev_node[agent];
		}
		path.push_back(start);
		std::reverse(path.begin(), path.end());
		return path;
	}

	std::vector<std::vector<Path>> edges;
	std::priority_queue<Agent> que;
	int start;
	int goal;
	int prev_node[N];
	bool defined[N];
};