def initialize_queue_and_visited(start):
    """
    Initialisiert die Warteschlange (queue) mit dem Startpunkt und das Set der besuchten Knoten (visited).
    
    Args:
    start (tuple): Startkoordinate im Format (x, y)
    
    Returns:
    tuple: Ein Tupel bestehend aus der Warteschlange (queue) und dem Set der besuchten Knoten (visited).
    """
    # raise NotImplementedError("initialize_queue_and_visited() is not implemented yet.")
    queue = [(start[0], start[1], [])]
    visited = {start}
    return (queue,visited)


def get_neighbors(x, y, maze, visited):
    """
    Berechnet die gültigen Nachbarn (Nachbarzellen) für einen gegebenen Punkt im Labyrinth.
    
    Args:
    x (int): Die X-Koordinate des aktuellen Punktes.
    y (int): Die Y-Koordinate des aktuellen Punktes.
    maze (list of list of int): Die 2D-Matrix, die das Labyrinth darstellt.
    visited (set): Das Set der besuchten Knoten.
    
    Returns:
    list: Eine Liste der Nachbarknoten, die besucht werden können.
    """

    returnval = []

    for i in (-1, 1):
        if y+i < 0 or y+i > (len(maze[0])-1):
            continue
        position = maze[y+i][x]
        # print(f"\nChecking: ({x},{y+i})")
        if not position and (y,x+i) not in visited:
            print(position)
            returnval.append((x, y+i))
            print(returnval)
        else:
            pass
    for i in (-1,1):
        if x+i < 0 or x+i > len(maze[0]):
            continue
        position = maze[y][x+i]
        # print(f"\nChecking: ({x+i},{y})")
        if not position and (x+i,y) not in visited:
            print(position)
            returnval.append((x+i, y))
            print(returnval)
        else:
            pass

    print(returnval)
    return returnval





def bfs_maze_solver(maze, start, goal):
    """
    Führt den Breadth-First Search (BFS) durch, um den kürzesten Pfad im Labyrinth zu finden.
    
    Args:
    maze (list of list of int): Die 2D-Matrix, die das Labyrinth darstellt.
    start (tuple): Startkoordinate im Format (x, y)
    goal (tuple): Zielkoordinate im Format (x, y)
    
    Returns:
    list or None: Der kürzeste Pfad als Liste von Koordinaten oder None, wenn kein Pfad gefunden wird.
    """    

    

    # Initialisiere die Warteschlange und die besuchten Knoten
    queue, visited = initialize_queue_and_visited(start)
    print(queue)

    while queue:
        x,y,path = queue.pop(0)

        if (x,y) == goal:
            return path+[(x,y)]
        else:
            n = get_neighbors(x,y,maze,visited)
            for new_x,new_y in n:
                visited.add((new_x, new_y))
                queue.append((new_x,new_y,path+[(x,y)]))

    return None

    
    # Füge hier den Code für BFS hinzu. 

# Beispiel-Labyrinth
maze = [
    [0, 1, 0, 0, 0],
    [0, 1, 0, 1, 0],
    [0, 0, 0, 1, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0]
]

start = (0, 0)
goal = (4, 4)

# Finde den kürzesten Pfad mit BFS
path = bfs_maze_solver(maze, start, goal)

# Gib das Ergebnis aus
if path:
    print("Path found:", path)
    exit()
else:
    print("No path found from start to goal.")