Added basic test
Refactoring and fixing printing bugs.