Add heuristic stuff to graph/problem libs

Needed for exercise 4
Start exercise 4

Still has an assert error somewhere for the greedy-best-first search.