Finish C++ code and main Python code