Odered Bayer Dithering

Hello!,
I am trying to apply ordered bayer dithering algo on a image and I am not able to get the result.
The algo is :–

foreach y
    foreach x
       oldpixel := pixel[x][y] + (pixel[x][y] * threshold_map_4x4[x mod 4][y mod 4])
       newpixel := find_closest_palette_color(oldpixel)
       pixel[x][y] := newpixel 

My code :–

void testApp::setup(){
     
    int factor = 0;
    float array[4][4] = {{1,9,3,11},{13,5,15,7},{4,12,2,10},{16,8,14,6}};
 
    for(int i = 0 ; i<4;i++){
        for(int j = 0 ; j<4;j++){
            array[i][j] = array[i][j]/17;
        }
    }

    img1.loadImage("hello.jpg");
    img1.resize(999,999);
    img1.update();
    
    img2 = img1;
    
    pix = img1.getPixels();
    pix2 = img2.getPixels();

    for(int k  = 0 ; k < 999 ; k++){
        for(int j = 0 ; j < 999*3 ; j+=3){
        
       factor = array[k%4][j%4];
        pix2[k*999 + j] +=  ( pix2[k*999 + j]  * factor );
        pix2[k*999 + j + 1] +=  ( pix2[k*999 + j + 1]  * factor );
        pix2[k*999 + j + 2] +=  ( pix2[k*999 + j + 2]  * factor );
            
            pix[k*999 + j] = ( pix2[k*999 +j] + 128 ) / 256 ;
            pix[k*999 + j+1] = ( pix2[k*999 +j+1] + 128 ) / 256 ;
            pix[k*999 + j+2] = ( pix2[k*999 +j+2] + 128 ) / 256 ;
                     
         }
      } 
    
   img1.update();
    
    }


//--------------------------------------------------------------
void testApp::update(){
    
   


}

//--------------------------------------------------------------
void testApp::draw(){

    img1.draw(0,0);
          
}

wikipedia :-- http://en.wikipedia.org/wiki/Ordered_dithering

What is wrong with my code ? Thanks!

I recall OpenCV had some methods to do that

I don’t want to go through the whole opencv docs ,it will simply make the code more complicated .Cant I do it simply by using just ofImage and the algo ?
Thanks!