Issue #1 fixed

TODO: Clean the code and add functionality to the new addition