Fixed bug in c++ code