Big debug output removed. Tests added. It works, but needs some restructuring.