import numpy as np

class node:

    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.neighbours = []
        self.parent = None
        self.g = 0
        self.f = 0
        self.h = 0

    def makeNeighbours(self, matrix):

        if self.x - 1 >= 0 and matrix[self.x - 1, self.y] != 3:
            self.neighbours.append([self.x - 1, self.y])

        if self.x + 1 < matrix.shape[0] and matrix[self.x + 1, self.y] != 3:
            self.neighbours.append([self.x + 1, self.y])

        if self.y + 1 < matrix.shape[1] and matrix[self.x, self.y + 1] != 3:
            self.neighbours.append([self.x, self.y + 1])

        if self.y - 1 >= 0 and matrix[self.x, self.y - 1] != 3:
            self.neighbours.append([self.x, self.y - 1])

    def calcHeuristic(self, goal):
        self.h = abs(self.x - goal.x) + abs(self.y - goal.y)

def Astar(A, start, goal):
    frontier = []

    objects = {}
    
    for i in range(A.shape[0]):
        for j in range(A.shape[1]):
            objects[i, j] = node(i, j)

    objects[0, 0].visited = True
    goal = objects[goal[0], goal[1]]
    start = objects[start[0], start[1]]

    frontier.append(start)
    
    open = []
    closed = []
    
    open.append(start)
    
    while open:
        open.sort(key=lambda o: o.f)
    
        current = open.pop(0)
    
        closed.append(current)
    
        current.makeNeighbours(A)
    
        if current == goal:
            break
    
        for next in current.neighbours:
            if objects[next[0], next[1]] in closed:
                continue
    
            if objects[next[0], next[1]] in open:
                new_g = current.g + 1
                if objects[next[0], next[1]].g > new_g:
                    objects[next[0], next[1]].g = new_g
                    objects[next[0], next[1]].parent = objects[current.x, current.y]
    
            else:
                objects[next[0], next[1]].g = objects[current.x, current.y].g + 1
                objects[next[0], next[1]].calcHeuristic(goal)
                objects[next[0], next[1]].parent = objects[current.x, current.y]
                open.append(objects[next[0], next[1]])
    
    path = []
    while current.parent:
        path.append([current.x, current.y])
        current = current.parent
    
    path.append([start.x, start.y])
    path.reverse()
    
    return path