Sh = 200
St = 10
WAREHOUSE_MAP = [
    [0, 0, 0, St, St, 0, 0, 0, 0],
    [0, St, 0, St, 0, St, 0, 0, 0],
    [0, St, 0, 0, 0, Sh, Sh, Sh, Sh],
    [0, 0, St, 0, 0, 0, 0, 0, 0],
    [0, 0, St, 0, 0, Sh, Sh, Sh, Sh],
    [0, 0, St, St, 0, 0, 0, 0, 0],
    [St, 0, 0, 0, St, Sh, Sh, Sh, Sh],
    [0, 0, St, St, St, 0, 0, 0, 0],
    [0, St, 0, 0, 0, Sh, Sh, Sh, Sh],
]


def a_star_search(start, end, agent_direction):

    open_nodes = []
    closed_nodes = []

    start_node = NodeAStar(agent_direction, None, start)
    start_node.g = start_node.h = start_node.f = 0
    end_node = NodeAStar(None, None, end)
    end_node.g = end_node.h = end_node.f = 0

    open_nodes.append(start_node)

    while len(open_nodes) != 0:
        current_node = open_nodes[0]
        current_node_index = 0
        for index, node in enumerate(open_nodes):
            if node.f < current_node.f:
                current_node = node
                current_node_index = index

        open_nodes.pop(current_node_index)
        closed_nodes.append(current_node)

        # Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę
        neighbour_nodes = []
        if current_node == end_node:
            path = []
            current = current_node
            while current is not None:
                #path.append(current.position)
                if current.action is not None:
                    current.action.reverse()
                    for each_action in current.action:
                        path.append(each_action)

                current = current.parent
            #path = path.pop()
            return path[::-1]
            #

        x = current_node.position[0]
        y = current_node.position[1]
        if x < 8:  # DOWN NEIGHBOUR
            if current_node.agent_direction == "right":
                actions = ["rotate_right", "move"]
            elif current_node.agent_direction == "left":
                actions = ["rotate_left", "move"]
            elif current_node.agent_direction == "up":
                actions = ["rotate_right", "rotate_right", "move"]
            elif current_node.agent_direction == "down":
                actions = ["move"]
            neighbour_nodes.append(
                NodeAStar("down", current_node, (x + 1, y), actions))

        if x > 0:  # UP NEIGHBOUR
            if current_node.agent_direction == "right":
                actions = ["rotate_left", "move"]
            elif current_node.agent_direction == "left":
                actions = ["rotate_right", "move"]
            elif current_node.agent_direction == "up":
                actions = ["move"]
            elif current_node.agent_direction == "down":
                actions = ["rotate_right", "rotate_right", "move"]
            neighbour_nodes.append(NodeAStar("up", current_node, (x - 1, y), actions))

        if y > 0:  # LEFT NEIGHBOUR
            if current_node.agent_direction == "right":
                actions = ["rotate_left", "rotate_left", "move"]
            elif current_node.agent_direction == "left":
                actions = ["move"]
            elif current_node.agent_direction == "up":
                actions = ["rotate_left", "move"]
            elif current_node.agent_direction == "down":
                actions = ["rotate_right", "move"]
            neighbour_nodes.append(NodeAStar("left", current_node, (x, y - 1), actions))

        if y < 8:  # RIGHT NEIGHBOUR
            if current_node.agent_direction == "right":
                actions = ["move"]
            elif current_node.agent_direction == "left":
                actions = ["rotate_left", "rotate_left", "move"]
            elif current_node.agent_direction == "up":
                actions = ["rotate_right", "move"]
            elif current_node.agent_direction == "down":
                actions = ["rotate_left", "move"]
            neighbour_nodes.append(NodeAStar("right", current_node, (x, y + 1), actions))
        for neighbour in neighbour_nodes:
            if len([closed_neighbour for closed_neighbour in closed_nodes if closed_neighbour == neighbour]) > 0:
                continue

            action_len = 0
            if current_node.action is not None:
                action_len = len(current_node.action)
            neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]] + action_len
            neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(
                neighbour.position[1] - end_node.position[1])
            neighbour.f = neighbour.g + neighbour.h

            if len([open_node for open_node in open_nodes if
                    neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0:
                continue

            open_nodes.append(neighbour)


class NodeAStar:
    def __init__(self, agent_direction, parent=None, position=None, action=None):
        self.agent_direction = agent_direction
        self.parent = parent
        self.position = position
        self.action = action
        self.g = 0
        self.h = 0
        self.f = 0

    def __eq__(self, other):
        return self.position == other.position