Basic breadth first pathfinding implemented