/*
 * Copyright (c) 1999 Johnny C. Lam.
 * All rights reserved.
 *
 * Permission to use, copy, modify, distribute and sell this software
 * and its documentation for any purpose is hereby granted without fee,
 * provided that the above copyright notice appear in all copies and
 * that both that copyright notice and this permission notice appear
 * in supporting documentation.  The author makes no representations
 * about the suitability of this software for any purpose.  It is
 * provided "as is" without express or implied warranty.
 */

// -*- C++ -*-

#ifndef STL_INTERNAL_GRAPHALG_H_INCLUDED
#define STL_INTERNAL_GRAPHALG_H_INCLUDED

template <class Graph, class EdgeWeight, typename WeightType>
WeightType
weight(const Graph & g, EdgeWeight edge_weight, WeightType init)
{
	typedef graph_traits<Graph> traits;

	typename traits::graph_type::const_edge_iterator e = g.e_begin();
	typename traits::graph_type::const_edge_iterator end = g.e_end();
	while (e != end) {
		init += edge_weight(*e);
		++e;
	}
	return init;
}

template <class Graph>
graph_traits<Graph>::weight_type
weight(const Graph &g)
{
	typedef graph_traits<Graph> traits;
	typedef typename traits::weight_type weight_type;
	
	weight_type acc = static_cast<weight_type>(0);
	typename traits::graph_type::const_edge_iterator e = g.e_begin();
	typename traits::graph_type::const_edge_iterator end = g.e_end();
	while (e != end) {
		acc += g.weight(*e);
		++e;
	}
	return acc;
}

///////////////////////////////////////////////////////////////////////////
//
// Dijkstra
//
// Computes single source shortest paths from source node for a
// non-negative graph, where for each node v,
//
//   dist[v] == cost of shortest path from source to v
//   tree[v] == predecessor edge of v, if != edge_type()
//
///////////////////////////////////////////////////////////////////////////

template <class Graph>
void
Dijkstra(const Graph &g,
		 const graph_traits<Graph>::node_type &start,
		 node_map<graph_traits<Graph>::weight_type> *dist,
		 node_map<graph_traits<Graph>::edge_type> *tree)
{
	typedef graph_traits<Graph>::weight_type weight_type;
	typedef graph_traits<Graph>::node_type node_type;
	typedef graph_traits<Graph>::edge_type edge_type;
	typedef numeric_limits<weight_type> num_limits;

	node_map<bool> visited(g.topology(), false);
	dynamic_priority_queue<weight_type, node_type, greater<weight_type> > PQ;
	
	/*
	 * The algorithm proceeds in such a way that the distances are
	 * estimated and the estimates are gradually improved.  The
	 * distance to the starting point is known (0).  For all other
	 * vertices, the worst possible estimate is entered.
	 */
	tree->init(g.topology());
	dist->init(g.topology(), num_limits::max());
	(*dist)[start] = 0;
	visited[start] = true;
	PQ.push(std::make_pair((*dist)[start], start));
	
	/* 
	 * In the next step, all vertices are extracted one by one from
	 * the priority queue, and precisely in the order of the estimated
	 * distance towards the starting vertex.  Obviously, the starting
	 * vertex is dealt with first.  No vertex is looked at twice.
	 */
	while (!PQ.empty()) {
		node_type u = PQ.top().second;		// extract vertex with minimum
		weight_type du = PQ.top().first;
		PQ.pop();
		
		/*
		 * Now, the distance estimates for all neighboring vertices of
		 * u are updated.  If the previous estimate of the distance
		 * between the current neighbor of u and the starting vertex
		 * (dist[v]) is worse than the distance between u and the
		 * neighboring vertex (wgt), the estimate is improved: this
		 * process is called relaxation.  In this case, the path from
		 * the starting vertex to the neighbor cannot be longer than
		 * (du + wgt).	In this case, u would have to be regarded as
		 * the predecessor of the neighbor.
		 */
		typename node_type::adj_edges_iterator e = u.adj_edges_begin();
		while (e != u.adj_edges_end()) {
			node_type v = e->target();
			weight_type dv = du + g.weight(*e);
			
			// relaxation
			if (!visited[v] || (*dist)[v] > dv) {
				visited[v] = true;
				if ((*dist)[v] == num_limits::max()) {
					PQ.push(std::make_pair(dv, v));
				} else {
					// improve estimate
					PQ.change_priority(v, dv);
				}
				(*dist)[v] = dv;
				(*tree)[v] = *e;
			}
			++e;
		}
	}
}

template <class Graph, class Visible>
void
DijkstraSubset(const Graph &g,
			   const graph_traits<Graph>::node_type &start,
			   const Visible &visible,
			   node_map<graph_traits<Graph>::weight_type> *dist,
			   node_map<graph_traits<Graph>::edge_type> *tree)
{
	typedef graph_traits<Graph>::weight_type weight_type;
	typedef graph_traits<Graph>::node_type node_type;
	typedef graph_traits<Graph>::edge_type edge_type;
	typedef std::set<node_type> node_set;
	typedef numeric_limits<weight_type> num_limits;
	
	node_map<int> out_degree(g.topology(), 0);
	Dijkstra(g, start, dist, tree);
	
	/*
	 * Find all the leaf nodes of the tree.
	 */
	typename Graph::node_iterator ni = g.n_begin();
	typename Graph::node_iterator n_end = g.n_end();
	while (ni != n_end) {
		edge_type e = (*tree)[*ni];
		++ni;
		if (e != edge_type()) {
			node_type parent = e.source();
			++out_degree[parent];
		}
	}
	node_set leaf_nodes;
	ni = g.n_begin();
	while (ni != n_end) {
		if (out_degree[*ni] == 0)
			leaf_nodes.insert(*ni);
		++ni;
	}
	/*
	 * Erase edges from the tree if it is a leaf edge and
	 * the leaf node isn't visible.
	 */
	typename node_set::iterator li = leaf_nodes.begin();
	typename node_set::iterator l_end = leaf_nodes.end();
	while (li != l_end) {
		node_type leaf = *li;
		node_type parent;
		while (!visible(leaf) && (*tree)[leaf] != edge_type()) {
			parent = (*tree)[leaf].source();
			(*dist)[leaf] = num_limits::max();
			(*tree)[leaf] = edge_type();	// erase predecessor
			--out_degree[parent];			// parent now has one less child
			if (out_degree[parent] > 0)
				break;
			leaf = parent;					// parent is now a leaf node
		}
		++li;
	}
	
	/*
	 * Adjust connections of nodes.
	 */
	ni = g.n_begin();
	while (ni != n_end) {
		node_type u = *ni;
		++ni;
		edge_type e = (*tree)[u];
		if ((e != edge_type()) && !visible(e.source())) {
			/*
			 * Compute path weight until parent is visible or has out_deg > 1.
			 */
			weight_type dl = g.weight(e);
			node_type parent = e.source();
			edge_type f = (*tree)[parent];
			while (!visible(parent) && (out_degree[parent] == 1) &&
				   (f != edge_type())) {
				dl += g.weight(f);
				parent = f.source();
				f = (*tree)[parent];
			}
			/*
			 * Find the smallest in_edge of u in g whose source is visible
			 * and isn't in the sub-tree below u.
			 */
			weight_type min_weight = num_limits::max();
			typename node_type::in_edges_iterator min_ei, ei;
			for (ei = u.in_edges_begin(); ei != u.in_edges_end(); ++ei) {
				node_type v = ei->source();
				if (u != v && visible(v) && g.weight(*ei) < min_weight) {
					/*
					 * Backtrack through tree to see if u is a parent of v.
					 */
					bool u_is_parent_of_v = false;
					edge_type f = (*tree)[v];
					while (!u_is_parent_of_v && (f != edge_type())) {
						v = f.source();
						f = (*tree)[v];
						if (u == v)
							u_is_parent_of_v = true;
					}
					if (!u_is_parent_of_v) {
						min_ei = ei;
						min_weight = g.weight(*ei);
					}
				}
			}
			/*
			 * If the smallest in_edge is smaller than the path weight
			 * through the invisible parents, then set the predecessor
			 * of u to the smallest in_edge.
			 */
			if (min_weight <= dl) {
				(*dist)[u] = (*dist)[min_ei->source()] + min_weight;
				(*tree)[u] = *min_ei;
				node_type parent = e.source();
				--out_degree[parent];
				/*
				 * Erase unneeded edges caused by the regrafting.
				 */
				node_type v = parent;
				while (!visible(v) && out_degree[v] == 0 &&
				       (*tree)[v] != edge_type()) {
					parent = (*tree)[v].source();
					(*dist)[v] = num_limits::max();
					(*tree)[v] = edge_type();	// erase latent leaf node
					--out_degree[parent];		// parent now has one less child
					v = parent;
				}
			}
		}
	}
}

template <class Graph, class OutputIterator>
void
Dijkstra(const Graph &g,
		 const graph_traits<Graph>::node_type &start,
		 node_map<graph_traits<Graph>::weight_type> *dist,
		 OutputIterator edge_inserter)
{
	typedef graph_traits<Graph>::weight_type weight_type;
	typedef graph_traits<Graph>::node_type node_type;
	typedef graph_traits<Graph>::edge_type edge_type;
	typedef numeric_limits<weight_type> num_limits;
	
	node_map<edge_type> tree;
	Dijkstra(g, start, dist, &tree);
	
	typename Graph::node_iterator n = g.n_begin();
	typename Graph::node_iterator end = g.n_end();
	while (n != end) {
		if (tree[*n] != edge_type())
			*edge_inserter++ = tree[*n];
		++n;
	}
}

template <class Graph, class Visible, class OutputIterator>
void
DijkstraSubset(const Graph &g,
			   const graph_traits<Graph>::node_type &start,
			   const Visible &visible,
			   node_map<graph_traits<Graph>::weight_type> *dist,
			   OutputIterator edge_inserter)
{
	typedef graph_traits<Graph>::weight_type weight_type;
	typedef graph_traits<Graph>::node_type node_type;
	typedef graph_traits<Graph>::edge_type edge_type;
	typedef numeric_limits<weight_type> num_limits;
	
	node_map<edge_type> tree;
	DijkstraSubset(g, start, visible, dist, &tree);
	
	typename Graph::node_iterator n = g.n_begin();
	typename Graph::node_iterator end = g.n_end();
	while (n != end) {
		if (tree[*n] != edge_type())
			*edge_inserter++ = tree[*n];
		++n;
	}
}

template <class Graph, class Visible, class OutputIterator>
void
DijkstraSubset(const Graph &g,
			   const graph_traits<Graph>::node_type &start,
			   const Visible &visible,
			   OutputIterator edge_inserter)
{
	node_map<graph_traits<Graph>::weight_type> dist;
	DijkstraSubset(g, start, visible, &dist, edge_inserter);
}

template <class Graph, class OutputIterator>
void
Dijkstra(const Graph &g,
		 const graph_traits<Graph>::node_type &start,
		 OutputIterator edge_inserter)
{
	node_map<graph_traits<Graph>::weight_type> dist;
	Dijkstra(g, start, &dist, edge_inserter);
}

#endif // STL_INTERNAL_GRAPHALG_H_INCLUDED
