import heapq

class graph: # _new_=regularconst vs _init_ =+ add-on manip. before creat.
    def __init__(self, width, height): 
        self.width = width
        self.height = height
        self.walls = []         # [] > list 
        self.weights = {}       # {} > dic['key' : 'value']- no dublicates 

    #checks if any pair of x,y is within our map/limits
    def in_bounds(self, id):
        (x, y) = id
        return 0 <= x < self.width and 0 <= y < self.height

    # checks and return true if param location has no obsticle on the way
    def passable(self, id):
        return id not in self.walls

    # returns all available adjacent (x,y) --in map and no walls
    def neighbors(self, id):
        (x, y) = id
        results = [(x+1, y), (x, y-1), (x-1, y), (x, y+1)]
        if (x + y) % 2 == 0: results.reverse()
        results = filter(self.in_bounds, results)
        results = filter(self.passable, results)
        return results

    # scales the effort to go from point A to B
    def cost(self, from_node, to_node):
        return self.weights.get(to_node, 1)


class PriorityQueue:
    def __init__(self):
        self.elements = []

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

    def put(self, item, priority):
        heapq.heappush(self.elements, (priority, item))

    def get(self):
        return heapq.heappop(self.elements)[1]

def reconstructPath(came_from, start, goal):
    current = goal
    path = [current]
    while current != start:
        current = came_from[current]
        path.append(current)
    path.reverse()
    return path

def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)

def aStarSearch(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put(start, 0)
    cameFrom = {}
    costSoFar = {}
    cameFrom[start] = None
    costSoFar[start] = 0

    while not frontier.empty():
        current = frontier.get()

        if current == goal:
            break

        for next in graph.neighbors(current):
            newCost = costSoFar[current] + graph.cost(current, next)
            if next not in costSoFar or newCost < costSoFar[next]:
                costSoFar[next] = newCost
                priority = newCost + heuristic(goal, next)
                frontier.put(next, priority)
                cameFrom[next] = current

    return cameFrom, costSoFar

example = graph(10, 10)
testStart = (0, 0)
testGoal = (5, 4)

(pathway, _ ) = aStarSearch(example, testStart, testGoal)
print(reconstructPath(pathway, testStart, testGoal))
