Rewrite CPP version
Completely rewrite the cpp version in to pure C.
Requires build flag "std=c99" to work.