Update problem.cpp

Fixed goalTest bug, temp Heuristic