Histogram i wyrównywanie histogramu

Witam wszystkich,

próbuję zrobić zadanie polegające na rysowaniu histogramu wczytanego
wcześniej obrazka, a później wyrównaniu tego histogramu (historam
equalization). Do momentu rysowania histogramu wszystko jest w
porządu, ale już nad wyrównywaniem siedzę i siedzę, i nie mogę nic
działającego wymyślić. Byłbym wdzięczny za jakąkolwiek pomoc w tym
temacie...

Rysowanie histogramu:

            Bitmap bmpHistogram = new Bitmap(pbHistogram1.Width,
pbHistogram1.Height);
            Bitmap bmpPrzed = new Bitmap(pictureBox1.Image);

            int[] tablica = new int[256];

            int max = 0;

            for (int x = 0; x < pictureBox1.Image.Width; x++)
            {
                for (int y = 0; y < pictureBox1.Image.Height; y++)
                {
                    // liczymy ilosc kolorow
                    Color cPixel = bmpPrzed.GetPixel(x, y);
                    int srednia = (cPixel.B + cPixel.G + cPixel.R) /
3;

                    tablica[srednia]++;
                }
            }

            for (int i = 0; i < 256; i++)
            {
                if (tablica[i] > max)
                {
                    max = tablica[i];
                }
            }

            Graphics g ;
            g = Graphics.FromImage(bmpHistogram);
            g.FillRectangle(Brushes.White, 0, 0, bmpHistogram.Width,
bmpHistogram.Height);

            double Procent = max / bmpHistogram.Height;
            double IlePx = bmpHistogram.Height * bmpHistogram.Width;
            for (int i = 0; i < 256; i++)
            {
                float secondHeight = pictureBox1.Height - (float)
(tablica[i] / Procent) - 49;

                g.DrawLine(Pens.Black, i, pictureBox1.Height, i,
secondHeight);
                pbHistogram1.Image = bmpHistogram;
                pbHistogram1.Invalidate();
            }

Wyrównanie histogramu:

            Bitmap bmpHistogram = new Bitmap(pbHistogram1.Width,
pbHistogram1.Height);
            Bitmap bmpPrzed = new Bitmap(pictureBox1.Image);

            int size = pbHistogram1.Height * pbHistogram1.Width;

            _srcHistogram = new int[256];

            int max = 0;

            for (int x = 0; x < pictureBox1.Image.Width; x++)
            {
                for (int y = 0; y < pictureBox1.Image.Height; y++)
                {
                    // liczymy ilosc kolorow
                    Color cPixel = bmpPrzed.GetPixel(x, y);
                    int srednia = (cPixel.B + cPixel.G + cPixel.R) /
3;

                    _srcHistogram[srednia]++;
                }
            }

            for (int i = 0; i < 256; i++)
            {
                if (_srcHistogram[i] > max)
                {
                    max = _srcHistogram[i];
                }
            }

            float[] lut = new float[256];
            int prevKey = 0;

            for (int k = 0; k < 256; k++)
            {
                prevKey += _srcHistogram[k];
                lut[k] = prevKey * ( max / size );
            }

            Graphics g;
            g = Graphics.FromImage(bmpHistogram);
            g.FillRectangle(Brushes.White, 0, 0, bmpHistogram.Width,
bmpHistogram.Height);

            double Procent = max / bmpHistogram.Height;
            double IlePx = bmpHistogram.Height * bmpHistogram.Width;
            for (int i = 0; i < 256; i++)
            {
                float secondHeight = pictureBox1.Height - (float)
(lut[i] / Procent) - 49;

                g.DrawLine(Pens.Black, i, pictureBox1.Height, i,
secondHeight);
                pbHistogram1.Image = bmpHistogram;
                pbHistogram1.Invalidate();
            }

Wzoruje się na artykule http://www.codersource.net/csharp_histogram_equalization.aspx, ale coś nie wychodzi :/
Mógłby mi ktoś pomóc? Będę bardzo wdzięczny