from itertools import product
from math import sqrt

class Pathfinding:
    def __init__(self, width, height, impassable):

        nodes = [[Gridnode(x, y) for y in range(height)] for x in xrange(width)]
        graph = {}

        for x, y in product(range(width), range(height)):
            node = nodes[x][y]
            graph[node] = []

            for i, j in product([-1, 0, 1], [-1, 0, 1]):
                if not (0 <= x + i < width):
                    continue
                if not (0 <= y + j < height):
                    continue
                if (x + i, y + j) in impassable:
                    continue
                graph[nodes[x][y]].append(nodes[x+i][y+j])

        self.graph, self.nodes = graph, nodes

    def heuristic(self, node, goal):
        D = 10
        dx = abs(node.x - goal.x)
        dy = abs(node.y - goal.y)
        return D * max(dx, dy)

    def move_cost(self, current, node):
       cross = abs(current.x - node.x) == 1 and abs(current.y - node.y) == 1
       return 19 if cross else 10

    def search(self, start, end):
        sx, sy = start[0], start[1]
        ex, ey = end[0], end[1]

        start = self.nodes[sx][sy]
        end = self.nodes[ex][ey]

        openset = set()
        closedset = set()
        current = start
        openset.add(current)
        start.parent = None

        while openset:
            current = min(openset, key=lambda o:o.g + o.h)

            if current == end:
                path = []

                while current.parent:
                    path.append(current)
                    current = current.parent
                path.append(current)
                return path[::-1]

            openset.remove(current)
            closedset.add(current)

            for node in self.graph[current]:
                if node in closedset:
                    continue
                if node in openset:
                    new_g = current.g + self.move_cost(current, node)
                    if node.g > new_g:
                        node.g = new_g
                        node.parent = current
                else:
                    node.g = current.g + self.move_cost(current, node)
                    node.h = self.heuristic(node, end)
                    node.parent = current
                    openset.add(node)
        
        return None


class Gridnode:
    def __init__(self, x, y):
        self.x, self.y = x, y
        self.g = 0
        self.h = 0
        self.parent = None

def main():
    from sandbox import SandboxMapMenu
    from cocos.director import director
    from cocos.scene import Scene

    director.init(3*96, 2*96, vsync=False, do_not_scale=True)
    director.run(Scene(SandboxMapMenu()))