Initial C++ implementation