Added basic test program
Updated headers to use its C++ variant