Bugfixing, and implementing test