HURRAY! Functional code at last. Applied fixes to other algo implementations.