#pragma once
#include <boost/graph/astar_search.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>


#include <math.h>


using boost::vecS;

// property bundle for vertices
class node
{
public:
	float x,y,z;
	void Set(float X,float Y, float Z)
	{
		x = X; y = Y; z = Z;
	}
};

// property bundle for edges
class edge
{
public:
	bool Requires_Jump;
	bool Passable;
	float Cost;
	void Set(float cost, bool jump, bool pass)
	{
		Cost = cost; Requires_Jump = jump; Passable = pass;
	}
};



typedef boost::adjacency_list<vecS,vecS,boost::bidirectionalS, node,edge> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef boost::graph_traits<Graph>::edge_descriptor Arc;

// 2D Euclidean distance calculator
template <class Graph_t, class CostType>
class Manhattan2D :  public boost::astar_heuristic<Graph_t, CostType>
{
public:
	typedef typename boost::graph_traits<Graph_t>::vertex_descriptor Vertex_t;

	Manhattan2D(Graph_t* g, Vertex_t goal): graph(g),m_Goal(goal){}
	CostType operator()(Vertex u)
	{
		CostType dx = (*graph)[m_goal].x - (*graph)[u].x;
		CostType dy = (*graph)[m_goal].y - (*graph)[u].y;
		return std::sqrt(dx * dx + dy * dy);
	}
private:
	Graph_t* graph;
	Vertex_t m_Goal;
}; 



struct found_goal {}; // exception for termination

// visitor that terminates when we find the goal
template <class Vertex_t>
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
  astar_goal_visitor(Vertex_t goal) : m_goal(goal) {}
  template <class Graph_t>
  void examine_vertex(Vertex_t u, Graph_t& g) {
    if(u == m_goal)
      throw found_goal();
  }
private:
  Vertex_t m_goal;
};
typedef astar_goal_visitor<Vertex> visitor_t;