// -------------------------------------------------------------------------
// Filename:	astar.h
// Version:		1.24 
// Date:		2002/03/08
// Purpose:		Provide template for a* algorythm
// (c) T.Frogley 1999-2002
// -------------------------------------------------------------------------

#ifndef ASTAR_H
#define ASTAR_H

#ifdef _MSC_VER
    //identifier was truncated to '255' characters in the browser information
    #pragma warning (disable : 4786)
#endif

//include standard library code
#include <vector>
#include <deque>
#include <functional>
#include <algorithm>
#include <limits>

#ifdef ASTAR_STATS
	#include <time.h>
	#include <iostream>
#endif

namespace astar
{
	//from <vector>
	using std::vector;

	//from <deque>
	using std::deque;

	//from <functional>
	using std::binary_function;
	using std::greater;

	//from <algorithm>
	using std::pop_heap;
	using std::push_heap;

	//max should be in <algorithm>
	//see: http://www.sgi.com/Technology/STL/max.html
	
	//unfortunatly the Microsoft compiler\headers arn't standard compliant
	//see: http://x42.deja.com/getdoc.xp?AN=520752890

	//thus:	
	#ifdef _MSC_VER
	
		#ifdef max 
		#undef max
		#endif

		template<class T> inline
		const T& max(const T& a, const T& b)
		{ return (a>b)?a:b; }
	#else
		using std::max;
	#endif

	//node = class encapsulating a location on the graph
	
	/* example node class for use with astar,
	   minimal interface - 
	   note it is recomended that in most cases nodes are implemented as pointers to data,

	struct example_node{

		//default, & copy constructor, &
		//assignment operator should be available
		
		//example_node::iterator
		//for fetching connected nodes, and costs

		struct iterator{

			//copy constructor and assignment operator should be available

			typedef double cost_type;	//typedef required, must be scalar type

			example_node value()const;	//node
			cost_type cost()const;		//cost/distance to node
			
			iterator& operator++();		//next node
			
			bool operator!=(iterator v);//used by search

		};

		//Get first, and past-end iterators
		iterator begin()const;
		iterator end()const;

		//equality operator, required
		//note: fuzzy equality may be useful
		bool operator==(const xynode b);
	};

	*/

	//heuristic = binary functor, estimates of cost from node A to node B

	//use this heuristic when costs don't apply
	template<class T>
	struct no_heuristic{
		//heuristic();
		typename T::iterator::cost_type operator()(const T a, const T b)
		{ return 1; }
	};

	//some useful template functions for creating heuristics for movement on a 2d plane
	//reference: http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html

	//The standard heuristic is the Manhattan distance. 
	//Look at your cost function and see what the least cost is 
	//for moving from one space to another. 
	//The heuristic should be cost times manhattan distance:
	template<class T> inline
	const T manhattan_distance(const T& x1, const T& y1, const  T& x2, const T& y2)
	{
		return (abs(x1-x2)+abs(y1-y2));
	}
	
	//If on your map you allow diagonal movement, then you need a different heuristic. 
	//The Manhattan distance for (4 east, 4 north) will be 8. 
	//However, you could simply move (4 northeast) instead, so the heuristic should be 4. 
	//This function handles diagonals:
	template<class T> inline
	const T diagonal_distance(const T& x1, const T& y1, const  T& x2, const T& y2)
	{
		return (max(abs(x1-x2),abs(y1-y2)));
	}

	//If your units can move at any angle (instead of grid directions), 
	//then you should probably use a straight line distance:
	template<class T> inline
	const T straight_distance(const T& x1, const T& y1, const T& x2, const T& y2)
	{
		T dx = (x1-x2);
		T dy = (y1-y2);
		return ( sqrt(dx*dx + dy*dy) );
	}

	//One thing that can lead to poor performance is ties in the heuristic. 
	//When several paths have the same f value, they are all explored, even 
	//though we only need to explore one of them. To solve this problem, we 
	//can add a small tie-breaker to the heuristic. 
	//This tie breaker also can give us nicer looking paths:
	//x1,y1 = start position
	//x2,y2 = current position
	//x3,y3 = target position
	template<class T> inline
	const T amits_modifier(const T& x1, const T& y1, const T& x2, const T& y2, const T& x3, const T& y3)
	{
		T dx1 = x2 - x3;
		T dy1 = y2 - y3;
		T dx2 = x1 - x3;
		T dy2 = y1 - y3;
		T cross = dx1*dy2 - dx2*dy1;
		if( cross<0 ) cross = -cross;
		return cross;
	}

	//node_link (astar implementation helper class)
	//wraps up a node with movement cost / heuristic info 
	//and an index to its parent node in the nodes list
	template<class T>
	struct node_link{
		typedef typename T::iterator::cost_type scalar;

		node_link(){}

		node_link(T n, scalar g, scalar h, int p=-1):
			myNode(n),
			myG(g),
			myH(h),
			myParent(p)
			{ }

		inline bool operator>(const node_link<T> &b)const
			{ return (myG+myH > b.myG+b.myH); }

		T myNode;
		scalar myG, myH;
		int myParent;
	};

	//binary_lookup functor (astar implemetatoin helper class)
	//	bfn : binary functor to pass value to
	//	key	: key to container
	//	con	: random access container, must support [ key ]
	//??? Why doesn't this work with c-style arrays ???
	template<class bfn, class key, class con>
	class binary_lookup : public binary_function<key, key, typename bfn::result_type> {
		
		public:
			//constructor, take a copy of functor, 
			//and keep a reference to the container
			binary_lookup(const bfn& f, con& _c):
				fn(f),
				c(_c)
			{ }
		
			//look up two values in contianer from keys a and b,
			//pass values to binary functor, and return result
			result_type operator()(
				const key& a, 
				const key& b	)
			{ return fn(c[a], c[b]); }

		protected:
			bfn fn;
			con& c;
	};

	//configuration info for astar algorythm
	//also used to return some additional information about the finished search
	template<class nodeType>
	struct config{
		typedef typename nodeType::iterator::cost_type scalar;
		
		//construct with sensable defaults / empty results
		config():
		node_limit(std::numeric_limits<unsigned int>::max()),
		cost_limit(std::numeric_limits<scalar>::max()),
		result_nodes_opened(0),
		result_nodes_pending(0),
		result_nodes_examined(0),
		route_length(0),
		route_cost(0)
		{ }
		
		//configuration variables

		//node_limit
		//set this to restrict the number of nodes astar will open
		//has the effect of limiting the amount of time spent searching
		unsigned int node_limit;	

		//cost limit
		//set this to restrict the maximum distance considered 
		//acceptable for a route
		//if astar cannot find a shorter route than this it will fail
		scalar cost_limit;
		
		//result variables
		
		//result_nodes_examined
		//astart sets the to the total number of nodes it 
		//'looked at'  when the search terminated
		unsigned int result_nodes_examined;

		//result_nodes_pending
		//astar sets this to the number of nodes still waiting 
		//to be examined when the search terminated
		unsigned int result_nodes_pending;

		//result_nodes_opened
		//astar sets this to the total number of nodes it fetched
		//should equal pending + examined
		unsigned int result_nodes_opened;

		//route_length
		//astar sets this to equal the total number of nodes
		//used to construct the returned route
		unsigned int route_length;

		//route_cost
		//astart sets the to equal the total "cost" of 
		//the returned route
		scalar route_cost;
	};

	//astar algorithm, as template, 
	//find a path from a to b, 
	//using the given heuristic, 
	//places results in container [ any class that can push_front( nodeType ) ]
	//returns flase if no route exists
	//returns true if a complete route is found
	//returns true if it exceeds node_limit, but a partial route is found
	//returns false (aborts with a partial route) if it exceeds cost_limit
	template<class heuristicFunctor, class nodeType, class container>
	bool astar(const nodeType a, const nodeType b, container &results, heuristicFunctor heuristic, config<nodeType> &cfg)
	{			 
		#ifdef ASTAR_STATS
		clock_t time = clock();
		#endif

		typedef node_link<nodeType> node;
		typedef vector<int> index_container;
		typedef deque<node> node_container;
		typedef typename nodeType::iterator node_iterator;
		typedef typename nodeType::iterator::cost_type scalar;

		node_container nodes;		//all nodes opened
		index_container pending;	//sorted index to pending nodes
		index_container done;		//unsorted index to nodes already explored
		index_container::iterator j;
		index_container::const_iterator k;
		int index;
		bool complete = false;
		
		//reserve space in index vectors to avoid reallocation
		if (cfg.node_limit != std::numeric_limits<unsigned int>::max()){
			pending.reserve( cfg.node_limit / 2 );
			done.reserve( cfg.node_limit / 2 );
		}

		//create the indirect comparison object
		greater<node> aFunctor;
		binary_lookup<
			greater<node>, 
			int, 
			node_container
		> sort_index_object(aFunctor, nodes);

		//tempory storage for 'working' node data
		node current;
		nodeType next;

		//stick the fist node into the list, 
		//and its index into the pending list
		nodes.push_back(node( a, 0, heuristic(a,b), -1 ));
		pending.push_back(nodes.size()-1);

		do{
			
			//get top rated node
		    index = pending.front();
			current = nodes[index];		
			
			//remove it from pending list
			pop_heap( pending.begin(), pending.end(), sort_index_object );
			pending.pop_back();

			//stick it in the list of examined nodes
			done.push_back(index);

			//found target?
			complete = current.myNode==b;
			if (complete) break;

			//failed (based on distance)
			if (current.myG+current.myH>cfg.cost_limit) break;
			
			//for each node connected to the current one
			node_iterator i(current.myNode.begin());
			const node_iterator end(current.myNode.end());
			for (;i!=end;++i){
				next = i.value();

				//!!! the client code might have a faster 
				//!!! way of checking if the node had already been visited

				//search pending list for "the same" node
				j=pending.begin();
				k=pending.end();
				for(;j!=k;++j){
					if (nodes[*j].myNode==next){
						break;
					}
				}

				//not in the pending list
				if (j==k){

					//search list of already explored nodes for "the same" node
					j=done.begin();
					k=done.end();
					for(;j!=k;++j){
						if (nodes[*j].myNode==next){
							break;
						}
					}
						
					//not in done list (or pending list)
					if (j==k){

						//add to pending list						
						nodes.push_back( node(next, i.cost()+current.myG, heuristic(next, b), index ) );
						pending.push_back( nodes.size()-1 );
						push_heap( pending.begin(), pending.end(), sort_index_object );

					}
				}
				else{
					
					//its in the pending list, but is this a better version ?
					if (i.cost()+current.myG<nodes[*j].myG){

						//replace node with this one
						nodes[*j] = node(next, i.cost()+current.myG, nodes[*j].myH, index );
						
						// This is allowed but it's not obvious why:
						// see: http://theory.stanford.edu/~amitp/GameProgramming/path.cpp
                        push_heap( pending.begin(), j+1, sort_index_object );

					}
				}
			}
		}while (!pending.empty() && nodes.size()<cfg.node_limit);

		#ifdef ASTAR_STATS		
		clock_t elapsed1 = clock() - time;
		#endif

		cfg.route_length = 0;
		
		//did not exit because a route was found
		if (!complete){
			
			//ran out of time
			if (nodes.size()>=cfg.node_limit && !pending.empty()){

				//best potential
				current = nodes[pending.front()];

				//search list of already explored nodes for "better" compromise
				j=done.begin();
				k=done.end();
				for(;j!=k;++j){
					if (current.myH>nodes[*j].myH){
						current = nodes[*j];
					}
				}

				//partial routes should not be considered a failure
				complete = true;
			}
		}

		#ifdef ASTAR_STATS
		elapsed1 = clock() - time;			
		#endif

		//store route length 
		//(including estimate of remaining distance for partial routes)
		cfg.route_cost = current.myG+current.myH;

		//store results of search in "container"
		++cfg.route_length;			
		results.push_front(current.myNode);

		while(current.myParent!=-1){
			current = nodes[current.myParent];
			results.push_front(current.myNode);

			++cfg.route_length;
		}

		//store stats for calling code
		cfg.result_nodes_opened = nodes.size();
		cfg.result_nodes_pending = pending.size();
		cfg.result_nodes_examined = done.size();

		//report stats
		#ifdef ASTAR_STATS
		clock_t elapsed2 = clock()-time;
		std::cout << "astar stats\n";
		std::cout << "ticks:\t\t" << elapsed2 << " (" << elapsed1 << ", " << elapsed2-elapsed1 << ")\n";
		std::cout << "(seconds):\t" << (float)elapsed2/CLOCKS_PER_SEC << "\n";		
		std::cout << "route length:\t" << cfg.route_length << " nodes (" << cfg.route_cost << " units)\n";
		std::cout << "nodes examined:\t" << cfg.result_nodes_examined << "\n";
		std::cout << "nodes pending:\t" << cfg.result_nodes_pending << "\n";
		std::cout << "(total):\t" << cfg.result_nodes_opened << "\n";	
		#endif

		return complete;
	}//void astar(...);

}//namespace astar

#endif