#include <stdlib.h>
#include <time.h>

#include <iostream>
#include <deque>

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

// undefine this to test without bundled properties
#define __USE_BUNDLED__

/** 
* Singleton design pattern is used to
* to make an instance of a particular class unique.
*/
template< typename X > class Singleton
{
public:
	inline static X& getInstance()
	{
		static X x;

		return x;
	}

protected:
	Singleton( void ) { }
	~Singleton( void ) { }
	Singleton( const Singleton& ) { }
	inline Singleton& operator = ( const Singleton& x )
	{ 
		return x;
	}
};


/***
 * @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.
 ***/
template< class Graph, class Vertex, class Edge, class VertexIterator, class EdgeIterator > class PathRecorder
{
	typedef typename boost::property_map< typename Graph, boost::vertex_index_t >::type VertexId_PMap;
	typedef boost::iterator_property_map< typename std::vector< Vertex >::iterator, VertexId_PMap, typename Vertex, typename Vertex & > PredMap;
	typedef boost::iterator_property_map< typename std::vector< Edge >::iterator, VertexId_PMap, typename Edge, typename Edge & > PredEdgeMap;

public:
	/**
	 * @desc
	 * Update an entry inside a map
	 **/
	void put( Vertex target, Vertex src )
	{
		boost::put( predMap, target, src );
	}

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

	/**
	 * @desc
	 * Get predecessor (ancestor) of target
	 **/
	Vertex getPredecessor( Vertex target ) const
	{
		return boost::get( predMap, target );
	}

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

	/**
	 * @desc
	 * Get edge from ancestor to sibling (if such edge exists)
	 **/
	Edge PathRecorder :: getEdgeToSibling( Vertex ancestor ) const
	{
		VertexIterator i, end;

		for( boost::tie( i, end ) = boost::vertices( g ); i != end; ++i )
		{
			Vertex pred = boost::get( predMap, *i );

			if( pred == ancestor )
			{
				return boost::get( predEdgeMap, *i );
			}
		}

		return Edge();
	}

	/***
	 * @desc reset path recorder for
	 * another search
	 ***/
	virtual void reset( void )
	{
		edges.assign( edges.size(), Edge() );
		vertices.assign( vertices.size(), Vertex() );
	}
protected:
	PathRecorder( const Graph &graph ) : g( graph ), 
		vertices( std::vector< Vertex >( boost::num_vertices( g ) ) ),
		edges( std::vector< Edge >( boost::num_vertices( g ) ) ),
		vertex_id( boost::get( boost::vertex_index, g ) ),
		predMap( vertices.begin(), vertex_id ),
		predEdgeMap( edges.begin(), vertex_id )
	{
	}

	const Graph g;

	~PathRecorder( void ) { }
	PathRecorder( const PathRecorder & ) { }

	std::vector< Vertex > vertices;
	std::vector< Edge > edges;
	VertexId_PMap vertex_id;

	PredMap predMap;
	PredEdgeMap predEdgeMap;
};

/**
 * @desc
 * Terminator is a class which simply tells DFS
 * to stop once the target vertex is reached.
 **/
template< class Vertex, class Graph > struct Terminator
{
	Terminator( Vertex v ) : destination( v )
	{
	}

	bool operator()( Vertex v, const Graph &g )
	{
		return ( v == destination );
	}

private:
	const Vertex destination;
};

#ifdef __USE_BUNDLED__
/*** 
 * Vertex Structure (bundled properties)
 * This is empty, but in original src code
 * this has some data inside.
 ***/
struct Clip_t
{
	Clip_t( void ) { }
};

/*** 
 * Edge Structure (bundled properties)
 * This is empty, but in original src code
 * this has some data inside.
 ***/
struct Transition_t
{
	Transition_t( void ) { }

	Transition_t( double d ) : distance( d )
	{
	}

	double distance;
};
#endif

/*** 
 * Type defines for our graph ( Called MotionGraph )
 ***/
#ifdef __USE_BUNDLED__
typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS, Clip_t, Transition_t > MotionGraph_t;
#else
typedef boost::adjacency_list< boost::listS, boost::vecS, boost::bidirectionalS > MotionGraph_t;
#endif

typedef boost::graph_traits< MotionGraph_t >::vertex_iterator ClipIterator;
typedef boost::graph_traits< MotionGraph_t >::vertex_descriptor Clip;
typedef boost::graph_traits< MotionGraph_t >::out_edge_iterator TransitionIterator;
typedef boost::graph_traits< MotionGraph_t >::edge_descriptor Transition;

/*** 
 * Singleton graph structure so we can access it anywhere we like
 * getBase is a way of passing the graph to BGL algorithm templates
 * if you pass this class directly templates will not compile due
 * to constructors and destructors being protected.
 ***/
class MotionGraph : public Singleton< MotionGraph >, public MotionGraph_t
{
	friend Singleton< MotionGraph >;
public:
	MotionGraph_t *getBase( void )
	{
		return static_cast< MotionGraph_t * >( &getInstance() );
	}
protected:
	~MotionGraph() { }
	MotionGraph() { }
	MotionGraph( const MotionGraph & ) { }

};

/**
@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;

	template < class Edge, class Graph > void operator()( Edge e, const Graph &g );
};

/**
@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;

	template < class Edge, class Graph > void operator()( Edge e, const Graph& g );
};

/*** 
 * Quick test implementation of PathRecorder interface.
 ***/
class TestPathRecorder : public PathRecorder< MotionGraph_t, Clip, Transition, ClipIterator, TransitionIterator >, public Singleton< TestPathRecorder >
{
	friend class Singleton< TestPathRecorder >;
public:
	template< typename Container > void search( Container &agenda, Clip start, Clip end )
	{
		std::vector< unsigned short > c( boost::num_vertices( g ) );

		reset();

		typedef boost::property_map< MotionGraph_t, 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;

		boost::depth_first_visit(
			g,
			start,
			boost::make_dfs_visitor( boost::make_list( TestPredRecorder(), TestForwardEdge() ) ),
			ColourMap( c.begin(), vertex_id ),
			Terminator< Clip, MotionGraph_t >( end )
			);

		getPath( agenda, start, end );
	}

	template< typename Container > void getPath( Container &agenda, Clip start, Clip end ) const
	{
		Transition cur = boost::get( predEdgeMap, end );

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

			cur = boost::get( predEdgeMap, boost::source( cur, g ) );
		}

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

protected:
	TestPathRecorder( void ) : PathRecorder( *MotionGraph::getInstance().getBase() ) { }
	~TestPathRecorder( void ) { }
	TestPathRecorder( const TestPathRecorder &mpr ) : PathRecorder( mpr.g ) { }
};

/**** Definitions of methods for visitor classes declared before TestPathRecorder ****/

template < class Edge, class Graph > void TestPredRecorder :: operator()( Edge e, const Graph &g )
{
	// simplified method, simply overwrite
	TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
	TestPathRecorder::getInstance().put( boost::target( e, g ), e );
}

template < class Edge, class Graph > void TestForwardEdge :: operator()( Edge e, const Graph &g )
{
	// simplified method, simply overwrite
	TestPathRecorder::getInstance().put( boost::target( e, g ), boost::source( e, g ) );
	TestPathRecorder::getInstance().put( boost::target( e, g ), e );
}

/******************************/

/*** Roll N-sided die ***/
int roll( int sides )
{
	return rand() % sides;
}

/*** Roll N-sided die so that result is different to prev ***/
int rollUnique( int sides, int prev )
{
	int r = roll( sides );

	while( r == prev )
	{
		r = roll( sides );
	}

	return r;
}

/*** Choose a random src and target and attempt to find a path ***/
void randomTest( void )
{
	std::deque< Clip > mg;

	int start = rand() % boost::num_vertices( MotionGraph::getInstance() );
	int end = rand() % boost::num_vertices( MotionGraph::getInstance() );

	TestPathRecorder::getInstance().search( mg, start, end );

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

	std::cout << "Path from " << start << " to " << end << ":" << std::endl;

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

int main( void )
{
	srand( time( NULL ) );

	// randomly choose number of vertices
	int vertices = roll( 500 );

	// add vertices
	for( int i = 0; i < vertices; ++i )
	{
#ifdef __USE_BUNDLED__
		boost::add_vertex( Clip_t(), MotionGraph::getInstance() );
#else
		boost::add_vertex( MotionGraph::getInstance() );
#endif
	}

	// for each vertex V choose a random number of edges and connect them to random vertices (excluding the V itself)
	for( int i = 0; i < vertices; ++i )
	{
		int edges = roll( vertices - 1 );

		for( int j = 0; j < edges; j++ )
		{
			int target = rollUnique( vertices, i );

#ifdef __USE_BUNDLED__
			boost::add_edge( i, target, Transition_t(), MotionGraph::getInstance() );
#else
			boost::add_edge( i, target, MotionGraph::getInstance() );
#endif
		}
	}

	// run a random path finding test for half the number of vertices
	for( int i = 0; i < vertices / 2; i++ )
	{
		randomTest();
	}

	return 0;
}