Minor algorithm improvements

Still have the bug with > 850 nodes...