Add manhattan distance heuristic function