Adding tests + cleanup
Simplifying the algorithm