Fixed algorithm efactoring bugs