Converted c library into c++. Needs some work