Cleaned code a bit.
Extracted algorithm from commit 73086f into separate class, corrected it.