Improvement to this code

[GaneshShenoy]

1. Add gTest (unit test) to improve your code test ability.
2. Add CMakeList to compile your code with any compiler
3. As you are working with .cpp i suggest to use classes instead of regular functions. and also add .h/.hpp files.