Rewrite in c++
Update documentation