Started converting Astar algorithm