Cpp version of improved Block Match Code