Bugfixes and cleaned code from Benjiro

#Currently Testing#