Added C++ implementation

Added C++ side of the implementation, and some tests.