Improve and add tests. Add alternative find shortest path method.