Update problem.cpp

updated heurisitc