Fix for previous commit and move function to header

Somehow I managed to introduce a syntax error with the setreg change so better fix that. 

Also the slight refactoring I mentioned on IRC.