Refactoring, bugfixes, added some commented debug output