Added a manual test, fixed several bugs.

Warning: some debug output is left in C++ code.