Debugged C++ version, ready to merge