Refactored code, ported changes from (dead) C++ branch, minor fixes