Code Cleanup for issue #93, should fix #110 as well

Hi, I had some time to cleanup the code from issue #93.
Comments or merges are welcome ;)