Added more tests, added debugging code for #46 and optimized.