// ダイクストラ法による最短距離計算及び最短路探索
// Copylight (C) 2009 mocchi  mocchi_2003@yahoo.co.jp
// License: Boost ver. 1.0

#ifndef IG_DIJKSTRA_H_
#define IG_DIJKSTRA_H_

#include <set>
#include <map>
#include <vector>
#include <cmath>
#include <limits>

/// ノードのグラフ情報を提供するためのインタフェース
template <typename I, typename T> struct INodeGraph{
	/// ノード p1 - p2間の距離を取得する。
	virtual I getDistance(T &p1, T &p2) = 0;

	/// ノード p1にリンクしているノード列を取得する。
	/// @param [in] p1 リンク元ノード
	/// @param [out] p1にリンクしているノード列
	virtual void getLinked(T &p1, std::vector<T> &link) = 0;
};

/// ダイクストラ法で、与えられた始点ノードとノードのグラフ情報から、始点からの各ノードの最短距離とノード毎の直前ノードを取得する。
/// @param [in] node1 始点ノード
/// @param [in] nodes_begin ノード列コンテナのbegin()
/// @param [in] nodes_end ノード列コンテナのend()
/// @param [in] nip ノードグラフ情報
/// @param [out] distance_map 距離マップ (キー:ノード　値:ノードの距離)
/// @param [out] previous_node ノード毎の直前ノード (キー:ノード 値:直前ノード) 始点ノードはキーに入れられていないため、previous_node.find(node1) == previous_node.end() となる。
template <typename I, typename T, typename IIter> void exec_dijkstra(T node1, IIter &nodes_begin, IIter &nodes_end, INodeGraph<I,T> &nip, std::map<T,I> &distance_map, std::map<T,T> &previous_node){
	T nextNode;
	double min;


	// キーがそのノード、値はたどってきた元のノード
	previous_node.clear();
	// キーがそのノード、値はそれまでの距離
	std::map<T,I> previousDistance;
	// キーがそのノード、値は確定したかどうか
	std::map<T,bool> previousVisited;

	// 一時データの初期化
	for (IIter itr = nodes_begin; itr != nodes_end; ++itr){
		T node = *itr;
		previousVisited[node] = false;
		previousDistance[node] = std::numeric_limits<I>::max();
	}

	nextNode = node1;
	previousDistance[nextNode] = 0;

	// 探索対象のノード
	std::set<T> toSearch;

	// 探索アルゴリズム本体
	do{
		// node_i は今回距離が確定したノード
		T node_i = nextNode;
		previousVisited[node_i] = true;

		// 確定したノードは探索対象から外す
		if (toSearch.count(node_i) > 0) toSearch.erase(node_i);

		min = std::numeric_limits<I>::max();

		// 確定ノードにリンクしているノード一覧を取得する。
		std::vector<T> linkids;
		nip.getLinked(node_i, linkids);

		// 確定ノードにリンクしている各ノードのうち、非確定ノードを探索対象に追加する。
		for (std::vector<T>::iterator itr = linkids.begin(); itr != linkids.end(); ++itr){
			T &node = *itr;
			if (previousVisited.count(node) > 0 && !previousVisited[node] && toSearch.count(node) == 0){
				toSearch.insert(node);
			}
		}

		// 探索対象の各ノードのうち、確定ノードiとリンクしているノードについて距離を計算し、
		// それらの中で距離が一番小さいノードを確定ノードにする。
		for (std::set<T>::iterator itr_j = toSearch.begin(); itr_j != toSearch.end(); ++itr_j){
			T node_j = *itr_j;
			bool isLinked = false;

			// ノードjが確定ノードiにリンクしているかどうかを判定する。判定結果がisLinkedとなる。
			for (std::vector<T>::iterator itr_i = linkids.begin(); itr_i != linkids.end(); ++itr_i){
				T &linkNodes = *itr_i;
				if (node_j == linkNodes){
					isLinked = true;
					break;
				}
			}


			// 確定ノードi経由の方が近い場合はノードjの最短経路を更新。
			I dist_i_d = previousDistance[node_i] + nip.getDistance(node_i, node_j);
			I dist_j_previous = previousDistance[node_j];
			I dist_j;
			if (isLinked && dist_i_d < dist_j_previous){
				previousDistance[node_j] = dist_i_d;
				previous_node[node_j] = node_i;
				dist_j = dist_i_d;
			}else{
				dist_j = dist_j_previous;
			}

			// 最短距離確定候補となるnextを保持。
			if (dist_j < min){
				min = dist_j;
				nextNode = node_j;
			}
		}
	}while(min < std::numeric_limits<I>::max());// すべてのノードへのルートが確定したらループから抜ける

	distance_map.swap(previousDistance);

	return;
}

/// 終点ノードと距離マップとノード毎の直前ノードから、始点-終点間の最短路を取得する。
/// @param [in] node2 終点ノード
/// @param [in] distance_map 距離マップ (キー:ノード　値:ノードの距離)
/// @param [in] previous_node ノード毎の直前ノード
/// @param [out] track 始点、終点間の最短路
template <typename I, typename T> void get_track(T node2, std::map<T,I> &distance_map, const std::map<T,T> &previous_node, std::vector<std::pair<T, I> > &track){
	//ノードn2から逆順にpreviousノードを辿り、経路を求める。
	T n = node2, p = node2;

	std::vector<T> backTrack;
	backTrack.push_back(n);
	std::map<T,T>::const_iterator fi;
	while((fi = previous_node.find(n)) != previous_node.end()){
		n = fi->second;
		backTrack.push_back(n);
	}
	track.clear();
	for (std::vector<T>::reverse_iterator itr = backTrack.rbegin(); itr != backTrack.rend(); ++itr){
		track.push_back(std::pair<T,I>(*itr, distance_map[*itr]));
	}

	return;
}


#endif // IG_DIJKSTRA_H_