Added way more memory efficient path finder
Added test for path finder