Refactored code, ported changes from (dead) C++ branch, fixed some bugs