Working version v2

Code is restructured. New methods and better comments.
I think this is the final one :)