rewrote in C++ for easier integrating