Bugfix

New bugs discovered, code refactored.

TODO refactor tests