working C++ single-threaded version