#!/usr/bin/python

import gridgraph
import priorityqueue
import vector

def heuristic_distance(a, b):
    return vector.distance_between(a.value, b.value)

def distance(a, b):
    return 1

def astar(start, end, d, hd):
    visited = set()

    dist = dict()
    dist[start] = 0

    hdist = dict()
    hdist[start] = hd(start, end)

    previous = dict()
    previous[start] = None

    queue = priorityqueue.Min()
    queue.push(start, 0)

    while queue.length > 0:
        n = queue.pop()

        if n == end:
            #reconstruct the path
            path = []
            while n in previous:
                path.insert(0, n)
                n = previous[n]
            path.insert(0, start)
            return path

        visited.add(n)

        #dist to each child
        for neighbor in n.children:
            if neighbor in visited:
                continue

            alt = dist[n] + d(n, neighbor)
            if neighbor not in dist or alt < dist[neighbor]:
                dist[neighbor] = alt
                previous[neighbor] = n
                #full distance calc:
                hdist[neighbor] = dist[neighbor] + hd(neighbor, end)
                queue.push(neighbor, hdist[neighbor])

    #end not found
    return None




def main():
    graph = gridgraph.GridGraph(20, 20)
    path = astar(graph.grid[0][0], graph.grid[19][19], distance, heuristic_distance)

    graph.marked[path[0].value] = "S "
    for n in range(1, len(path)-1):
        graph.marked[path[n].value] = "O "
    graph.marked[path[-1].value] = "E "

    print graph


if __name__ == "__main__":
    main()


OUTPUT:
/usr/bin/python2.7 /home/jesse/Development/Python/goog/playground.py
O - - - - - - - - - - - - - - - - - - -
O O O - - - - - - - - - - - - - - - - -
- - O O - - - - - - - - - - - - - - - -
- - - O - - - - - - - - - - - - - - - -
- - - O O O - - - - - - - - - - - - - -
- - - - - O O - - - - - - - - - - - - -
- - - - - - O O - - - - - - - - - - - -
- - - - - - - O - - - - - - - - - - - -
- - - - - - - O O O - - - - - - - - - -
- - - - - - - - - O O - - - - - - - - -
- - - - - - - - - - O - - - - - - - - -
- - - - - - - - - - O O O - - - - - - -
- - - - - - - - - - - - O - - - - - - -
- - - - - - - - - - - - O O - - - - - -
- - - - - - - - - - - - - O O - - - - -
- - - - - - - - - - - - - - O O O - - -
- - - - - - - - - - - - - - - - O - - -
- - - - - - - - - - - - - - - - O O O -
- - - - - - - - - - - - - - - - - - O O
- - - - - - - - - - - - - - - - - - - E

Process finished with exit code 0