Refactor, cleaner, comment, add tests. Solve Issue #18. General algorithm is more clear.