OptimalPathFindingException and some refactoring of algorithm