Find Shortest Path In 2d Array Python