Fixed algorithm such that it actually works now.
Minor changes.