Added some functions to start on the A* algorithm