Adding the C++ code, makefile, objects directory, and matlab code