Add methods to modify the Graph

* Add test coverage
* Limit the scope of nodes variable
Make PQueue functions consistent