Tweaked algorithm, added much functionality. Almost done.