Added cpp file and updated readme