pathfind algorithm implemented, still needs test with obstacles