A* algorithm + first draw function