Cleaning up hadlock's algorithm a bit and added a few optimizations.