Lots of small fixes.

- Added some testcode to main
- Fixed some bugs
- Added some debug code