Improved algorithm (and unfortunately reduced the efficiency to a worst
case scenario).
Added unit tests.
Added comments.