Optimized algorithm with SortedSet
Merge pull request #2 from eternalmatt/master

Optimized algorithm with SortedSet
-Thanks Matt much cleaner.