Rewrite from scratch of 005
Add C++ version of 005 for speed testing