Added heuristics a the A* search algorithm