Updated A* to get rid of infinite loop