Issue 25 + refactoring

Just a little refactoring and fixing issue #25