Fixed algorithm, added unit-tests