Added cpp code

Converted c# code to cpp