Ordered dithering - C# - color values per channel

See more:

C#

image-processing

Hey,
I am trying to implement the Ordered Dithering Algorithm. I have already somehow working code, but it is not returning a proper result bitmaps.

The function that I use takes 2 input values:
- the size of the threshold map (n)
- number of color values per channel (k)

I am mostly following this pseudocode:

I = GetPixel(x,y)
col = Floor((k-1)*I)
re = (k-1)*I - col
if (re >= Bn[x mod n, y mod n])
    col++
SetPixel(x,y,layers[col])

My code:

private float[,] dither2x2Matrix =
        new float[,] { { 1, 3 },
                    { 4, 2 } };

    private float[,] dither3x3Matrix =
        new float[,] { { 3, 7, 4 },
                    { 6, 1, 9 },
                     { 2, 8, 5 } };

    public BitmapImage OrderedDitheringApply(BitmapImage FilteredImage, int colorsNum, int thresholdSize)
    {
        Bitmap bitmap = ImageConverters.BitmapImage2Bitmap(FilteredImage);

        float[,] bayerMatrix;

        if (thresholdSize == 2)
        {
            bayerMatrix = new float[2, 2];
            for (int i = 0; i < 2; ++i)
                for (int j = 0; j < 2; ++j)
                    bayerMatrix[i,j] = dither2x2Matrix[i,j] / 5;
        }
        else
        {
            bayerMatrix = new float[3, 3];
            for (int i = 0; i < 3; ++i)
                for (int j = 0; j < 3; ++j)
                    bayerMatrix[i, j] = dither3x3Matrix[i, j] / 10;
        }

        for (int i = 0; i < bitmap.Width; ++i)
            for(int j = 0; j < bitmap.Height; ++j)
            {

                Color color = bitmap.GetPixel(i, j);
                float colorIntensity = color.GetBrightness();

                float tempValue = (float)(Math.Floor((double)((colorsNum - 1) * colorIntensity)));
                float re = (colorsNum - 1) * colorIntensity - tempValue;

                if (re >= bayerMatrix[i % thresholdSize, j % thresholdSize])
                    tempValue++;

                if(tempValue == 0)
                    bitmap.SetPixel(i, j, Color.FromArgb(0,0,0));
                else
                    bitmap.SetPixel(i, j, Color.FromArgb(255, 255, 255));
            }

        return ImageConverters.Bitmap2BitmapImage(bitmap);
    }

However when I use more than 2 color values per channel it returns weird, mostly whithish images. I assume there is something wrong, but I can't really see what.

What I have tried:

Have implemented working code, but it doesn't return proper results