#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <iostream>
#include <math.h>

using namespace boost;
using namespace std;

// define what a state is -- here it is a simple x,y position
struct State
{
	State() : x(0), y(0) {}
	State(int xi, int yi) : x(xi), y(yi) {}
	int x;
	int y;
	bool operator ==(const State& state)
	{
		return state.x == x && state.y == y;
	}
};

struct vertex_state_t { typedef vertex_property_tag kind; }; // custom property tag
struct found_goal {}; // exception for termination

// define vertex and edge properties
typedef property<vertex_color_t, default_color_type,
		property<vertex_rank_t, unsigned int,
		property<vertex_distance_t, unsigned int,
		property<vertex_predecessor_t, unsigned int,
		property<vertex_state_t, State> > > > > vert_prop;
typedef property<edge_weight_t, double> edge_prop;

// define the graph type
typedef adjacency_list<listS, vecS, undirectedS, vert_prop, edge_prop > mygraph_t;
typedef mygraph_t::vertex_descriptor vertex_t;
typedef mygraph_t::vertex_iterator vertex_iterator_t;
typedef property_map<mygraph_t, vertex_state_t>::type StateMap;

// define the visitor type -- this visitor is used to insert custom code into 
// the a-star search
class grid_visitor : public default_astar_visitor
{
public:
	grid_visitor(State &goal) : m_goal(goal)
	{
	}

	// callback used after popping a vertex off the stack in the a-star search
	void examine_vertex(vertex_t u, const mygraph_t& cg) {
		mygraph_t& g = const_cast<mygraph_t&>(cg);
		StateMap stateMap = get(vertex_state_t(), g);
		
		// check for goal
		State& curr = stateMap[u];
		if(curr == m_goal)
			throw found_goal();

		// find the children of this state
		vector<State> children;
		children.push_back(State(curr.x+1, curr.y));
		children.push_back(State(curr.x-1, curr.y));
		children.push_back(State(curr.x, curr.y+1));
		children.push_back(State(curr.x, curr.y-1));

		// check to see if these states already exist in the map
		for (size_t k=0; k< children.size(); k++)
		{
			// make sure this state is new (very inefficient this way)
			vertex_iterator_t vi, vend;
			for(tie(vi, vend) = vertices(g); vi != vend; ++vi)
				if(stateMap[*vi] == children[k])
					break;
			if(vi != vend) // not new
				add_edge(u, *vi, edge_prop(1.0), g);
			else { // new
				vertex_t v = add_vertex(vert_prop(white_color), g);
				stateMap[v] = children[k];
				add_edge(u, v, edge_prop(1.0), g);
			}
		}
	}
private:
	State& m_goal;
};

// define the a-star cost hueristic
class distance_heuristic : public astar_heuristic<mygraph_t, double>
{
public:
	distance_heuristic(StateMap& stateMap, State goal) : m_stateMap(stateMap), m_goal(goal) {}
	double operator()(vertex_t u)
	{
		double dx = m_goal.x - m_stateMap[u].x;
		double dy = m_goal.y - m_stateMap[u].y;
		return sqrt(dx*dx + dy*dy);
	}
private:
	StateMap& m_stateMap;
	State m_goal;
};

// main
int main(int argc, char* argv[])
{
	// define the goal
	State goal(3,3);

	// create the graph and add the initial state
	mygraph_t g;
	StateMap stateMap = get(vertex_state_t(), g);
	vertex_t start = add_vertex(vert_prop(white_color), g);
	stateMap[start] = State(0,0);
		
	try 
	{
		// create the a-star visitor
		grid_visitor vis(goal);

		// run the a-star algorithm
		astar_search(g, start, distance_heuristic(stateMap, goal), 
			visitor(vis).
			color_map(get(vertex_color, g)).
			rank_map(get(vertex_rank, g)).
			distance_map(get(vertex_distance, g)).
			predecessor_map(get(vertex_predecessor, g)));
	} 
	catch(found_goal fg) 
	{
		cout << "yay!" << endl;
	}
	return 0;
}