Starting port to C++
Finished C++ port. Added some tests