LitDev.Engines.FIP.ImageSDROMFilterGS C# (CSharp) Method

        public Bitmap ImageSDROMFilterGS(Bitmap OriginalImage)
        {
            Bitmap image = OriginalImage;
            FastPixel fpImage = new FastPixel(image);
            Bitmap image2 = new Bitmap(fpImage.Width, fpImage.Height);
            FastPixel fpImage2 = new FastPixel(image2);

            int[] thresholds = new int[4] { 20, 40, 60, 80 };
            int size = 3;

            int range = (int)Math.Floor(Convert.ToDouble(size / 2));

            for (int m = range; m < fpImage.Width - range; m++)
            {
                for (int n = range; n < fpImage.Height - range; n++)
                {

                    int[,] roi = new int[size, size];

                    int tmpj = 0;
                    int tmpi = 0;

                    int newValue = 0;

                    Color CPixel = fpImage.GetPixel(m, n);
                    int CP = (int)Convert.ToDouble((CPixel.R * 0.3) + (CPixel.G * 0.59) + (CPixel.B * 0.11));

                    for (int i = m - range; i < m + range + 1; i++)
                    {
                        for (int j = n - range; j < n + range + 1; j++)
                        {
                            Color pixel = fpImage.GetPixel(i, j);
                            Double p = Convert.ToDouble((pixel.R * 0.3) + (pixel.G * 0.59) + (pixel.B * 0.11));

                            roi[tmpi, tmpj] = (int)p;

                            tmpj++;
                        }

                        tmpi++;
                        tmpj = 0;
                    }

                    int[] roivector = new int[size * size - 1];

                    int tmp = 0;

                    for (int i = 0; i < roi.GetLength(0); i++)
                    {
                        for (int j = 0; j < roi.GetLength(1); j++)
                        {

                            if (i == range && j == range)
                            {

                            }
                            else
                            {
                                roivector[tmp] = roi[i, j];
                                tmp++;
                            }
                        }
                    }

                    System.Array.Sort(roivector);

                    int ROM = 0;
                    int sizeROI = roivector.Length;
                    int mid = sizeROI / 2 - 1;

                    Double a = Convert.ToDouble(roivector[mid]);
                    Double b = Convert.ToDouble(roivector[mid + 1]);
                    Double c = (a + b) / 2;

                    ROM = (int)c;

                    int[] ROD = new int[mid + 1];

                    for (int i = 0; i < mid; i++)
                    {
                        if (CP <= ROM)
                        {
                            ROD[i] = roivector[i] - CP;
                        }
                        else
                        {
                            ROD[i] = CP - roivector[roivector.GetLength(0) - 1 - i];
                        }
                    }

                    newValue = CP;

                    for (int i = 0; i < mid; i++)
                    {
                        if (ROD[i] > thresholds[i])
                        {
                            newValue = ROM;
                            break;
                        }
                    }

                    if (newValue > 255) newValue = 255;
                    if (newValue < 0) newValue = 0;

                    fpImage2.SetPixel(m, n, Color.FromArgb(255, newValue, newValue, newValue));
                }
            }

            fpImage.Unlock(false);
            fpImage2.Unlock(true);
            return image2;
        }