Added an 'optimized' cpp version, not yet functional though