cpp version almost done, about 4x time faster, yeeah