Implement first version of findShortestPath method. Improve tests.

* Make sure tests reset instances with every run (beforeEach call)
* Get rid of old commented out code