Lots of work, not so many bug fixes
Fixed memory leak
added a c++ version, I think it works