Changed to cpp file
Both version work fine. Need to add timing
Both versions works fine, 5x speedup