C++ version is now fully functional, but still slow

will work on finding out what is slowing it down later.