#include <iostream>
#include <deque>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>

/*** 
 * Type defines for our graph ( Called MotionGraph )
 ***/
typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS > MyGraph;
typedef boost::graph_traits< MyGraph >::vertex_descriptor MyVertex;
typedef boost::graph_traits< MyGraph >::edge_descriptor MyEdge;

/***
 * @desc
 * PathRecorder class is a template for creating
 * path recorders which utilise visitor concepts
 * in BGL. I.e. as a search algorithm runs,
 * PathRecorder is called to fill in a "predecessor map"
 * which records how to get to a specific vertex and stores
 * edges with their targets as keys.
 ***/
class PathRecorder
{
	typedef boost::property_map< MyGraph, boost::vertex_index_t >::type VertexId_PMap;
	typedef boost::iterator_property_map< std::vector< MyEdge >::iterator, VertexId_PMap, MyEdge, MyEdge & > PredEdgeMap;

public:
	/**
	 * @desc
	 * Update an entry inside a map
	 **/
	void put( MyVertex target, MyEdge e )
	{
		boost::put( predEdgeMap, target, e );
	}

	/**
	 * @desc
	 * Get MyEdge to target
	 **/
	MyEdge getEdge( MyVertex target ) const
	{
		return boost::get( predEdgeMap, target );
	}

	PathRecorder( const MyGraph &graph ) : g( graph ), 
		edges( std::vector< MyEdge >( boost::num_vertices( g ) ) ),
		Vertex_id( boost::get( boost::vertex_index, g ) ),
		predEdgeMap( edges.begin(), Vertex_id )
	{
	}
	~PathRecorder( void ) { }
	PathRecorder( const PathRecorder & ) { }
protected:
	const MyGraph g;
	std::vector< MyEdge > edges;
	VertexId_PMap Vertex_id;
	PredEdgeMap predEdgeMap;
};


/**
@desc
ForwardEdge is a visitor which tries to decide whether
to replace a newly discovered edge with the edge inside
a DFS tree (predecessor map).
Forward Edge is an edge (U,V) and both U, V have already
been discovered by DFS.
**/
struct TestForwardEdge : public boost::base_visitor< TestForwardEdge >
{
	typedef boost::on_forward_or_cross_edge event_filter;

	TestForwardEdge( PathRecorder &__pr ) : pr( __pr ) { }

	void operator()( MyEdge e, const MyGraph &g )
	{
		// simplified method, simply overwrite
		pr.put( boost::target( e, g ), e );
	}

	PathRecorder pr;
};

/**
@desc
PredRecorder adds a tree edge to predecessor map.
Tree edge is the first edge for pair of vertices
(U, V) that was ever discovered.
**/
struct TestPredRecorder : public boost::base_visitor< TestPredRecorder >
{
	typedef boost::on_tree_edge event_filter;

	TestPredRecorder( PathRecorder &__pr ) : pr( __pr ) { }

	void operator()( MyEdge e, const MyGraph &g )
	{
		// simplified method, simply overwrite
		pr.put( boost::target( e, g ), e );
	}

	PathRecorder pr;
};


void getPath( std::deque< MyVertex > &agenda, MyVertex start, MyVertex end, MyGraph &g, PathRecorder &pr )
{
	MyEdge cur = pr.getEdge( end );

	while( cur.m_eproperty )
	{
		agenda.push_front( boost::target( cur, g ) );
		agenda.push_front( boost::source( cur, g ) );

		cur = pr.getEdge( boost::source( cur, g ) );
	}

	if( agenda.empty() || agenda.front() != start )
	{
		agenda.clear();
	}
}

void search( std::deque< MyVertex > &agenda, MyVertex start, MyVertex end, MyGraph &g )
{
	std::vector< unsigned short > c( boost::num_vertices( g ) );

	typedef boost::property_map< MyGraph, boost::vertex_index_t >::type VertexId_PMap;
	VertexId_PMap vertex_id = boost::get( boost::vertex_index, g );
	typedef boost::iterator_property_map< std::vector< unsigned short >::iterator, VertexId_PMap, unsigned short, unsigned short & > ColourMap;

	PathRecorder pr( g );

	boost::depth_first_visit(
		g,
		start,
		boost::make_dfs_visitor( boost::make_list( TestPredRecorder( pr ), TestForwardEdge( pr ) ) ),
		ColourMap( c.begin(), vertex_id )
		);

	getPath( agenda, start, end, g, pr );
}

void test( MyGraph &g, MyVertex one, MyVertex two )
{
	std::deque< MyVertex > mg;

	search( mg, one, two, g );

	if( mg.empty() )
	{
		std::cout << "Destination unreachable" << std::endl;
		return;
	}

	std::cout << "Path from " << one << " to " << two << ":" << std::endl;

	for( int i = 0; i < mg.size(); ++i )
	{
		std::cout << mg.at( i ) << ( i != mg.size() - 1 ? " -> " : "\n" ); 
	}
}

int main( void )
{
	int vertices = 5;

	MyGraph g( vertices );

	typedef std::pair< MyVertex, MyVertex > Pair;

	Pair edges[] = {
		Pair( 0,1 ),
		Pair( 2,0 ),
		Pair( 3,2 ),
		Pair( 4,1 ),
		Pair( 0,3 ),
		Pair( 2,4 ),
		Pair( 3,4 ),
		Pair( 4,1 ),
		Pair( 2,1 ),
		Pair( 2,1 )
	};

	for( int i = 0; i < 10; ++i )
	{
		boost::add_edge( edges[i].first, edges[i].second, g );
	}

	test( g, 1, 4 );
	test( g, 4, 0 );
	test( g, 3, 0 );
	test( g, 2, 3 );
	test( g, 1, 0 );

	return 0;
}