How to implement dithering algorithm in c++?

I mean I want to implement dithering algorithm .

 

Dithering Algorithm :

I have a  bitmap matrix of an  image .   For example :      

                                                                                              [ 240          220  ]       ( color range: 0 -  255)

                                                                                              [ 240          150  ]

                                                                                              [100          30     ]3*3

 

 

and there is a dither matrix (It can be 2*2 and 4*4 )                            

                                                    for example :  [  0    2]

                                                                              [ 3     1] 2*2

Note :if I use 4*4 dither matrix the picture will be enlarged 16 times.                         

 

 

dither formula :   256 / ((2*2) + 1) = 51.2      Note: the 2*2 is the size of dither matrix. 

 

 

so now I want to divide all the elements of bitmap matrix to dither formula result,  

 the result matrix is :  

                                             [ 240/51.2                150/51.2 ]

                                             [  100/51.2                30/51.2  ]

 

 

 

                           the result matrix is :                                                                                                                     

                                      [  4          4 ]

                                      [ 4           2 ]                                                                                                                                                             [ 1           0 ]  3*2             

                                                                                                                                                                                                                      

 

because the dither matrix is 2*2 so each element of result matrix must be compared with dither matrix elements  so the result will be 6*4 matrix

 for example :  4 in result matrix is greater than all elements of dither matrix so the result will be :

                  the algorithm asnwer : 

                                                                                                                                                                                                   1     1              1      1

                 1     1              1      1

                 1     1              1      1

                 1     1              0      1

                 1     0              1      0

                 0     1              0      0      6*4

 

 

So I want to user to enter the bitmap matrix size and then ask user to insert bitmap matrix elements next ask user to enter the dither matrix  size then ask user to insert the dither matrix elements. at the result I want to display algorithm matrix as an answer matrix in above.

Please guide me.