Final version of path finder. Added tests and readme.