from math import sqrt
class PriorityQueue:
	def __init__(self):
		self.nlist = []
		self.plookup = {}

	def put(self, node, priority):
		if node not in self.nlist:
			self.nlist.append(node)
		self.plookup[node] = priority
		self.__sort()

	def __sort(self):
		self.nlist.sort(key=lambda node: self.plookup[node])

	def __contains__(self, node):
		return self.nlist.__contains__(node)

	def __getitem__(self, key):
		return self.nlist.__getitem__(key)

	def get(self):
		m = self.nlist[0]
		del self.nlist[0]
		del self.plookup[m]
		return m

	def empty(self):
		return len(self.nlist) == 0

class AStar:

	def solve(self, begin, end):
		#set up data structures for search
		glookup = {}
		pdlookup = {}
		openlist = PriorityQueue()
		closedlist = []

		openlist.put(begin,0)
		glookup[begin] = 0

		while not openlist.empty():
			currentNode = openlist.get()
			if currentNode == end:
				return self.__path(pdlookup,begin,end)

			closedlist.append(currentNode)

			self.__expandNode(currentNode,glookup, pdlookup, openlist, closedlist, end)

		return None

	def __path(self,pdlookup,begin,end):
		route = [end]
		node = end
		while node != begin:
			node = pdlookup[node]
			route.append(node)
		return list(reversed(route))

	def __expandNode(self,node, glookup, pdlookup, openlist, closedlist, end):
		for successor in self.get_successors(node):
			if successor in closedlist:
				continue

			tentative_g = glookup[node] + self.cost(node, successor)

			if successor in openlist and glookup[successor] <= tentative_g:
				continue

			pdlookup[successor] = node
			glookup[successor] = tentative_g

			openlist.put(successor, tentative_g + self.esteemed_cost(successor,end))

	def cost(self, node1, node2):
		raise "cost must be overwritten"

	def esteemed_cost(self, node, end):
		raise "esteemed_cost must be overwritten"

	def get_successors(self, node):
		raise "get_successors must be overwritten"



if __name__ == '__main__':
	#Testing
	def addnodes(*nodes):
		result = (0,0)
		for node in nodes:
			result = (result[0] + node[0], result[1]+node[1])
		return result

	class Test(AStar):
		def cost(self, node1, node2):
			return 1
		def esteemed_cost(self, node, end):
			return sqrt( (node[0]-end[0])**2 + (node[1]-end[1])**2 )

		def get_successors(self, node):
			return map(lambda offset: addnodes(offset, node), [(1,0),(0,1),(-1,0),(0,-1),(1,1),(1,-1),(-1,-1),(-1,1)])

	test = Test()
	print test.solve( (1,1), (3,5) )