Made a C++ templated version. Tested it a bit, but no unit tests.