ConcurrentDithering.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace Compression
{

    public class ConcurrentDithering
    {        static uint Interleave(uint x, uint y)
        {
            uint[] B = { 0x55555555, 0x33333333, 0x0F0F0F0F, 0x00FF00FF };
            int[] S = { 1, 2, 4, 8 };


            x = (x | (x << S[3])) & B[3];
            x = (x | (x << S[2])) & B[2];
            x = (x | (x << S[1])) & B[1];
            x = (x | (x << S[0])) & B[0];

            y = (y | (y << S[3])) & B[3];
            y = (y | (y << S[2])) & B[2];
            y = (y | (y << S[1])) & B[1];
            y = (y | (y << S[0])) & B[0];

            return x | (y << 1);
        }
        static uint Invert(uint x, byte bitCount)
        {
            uint y = 0;
            while (bitCount > 0)
            {
                y <<= 1;
                y |= x & 1;
                x >>= 1;
                bitCount--;
            }
            return y;
        }

        /*
         * 0 - 1x1 (no dithering while using dithering)
         * 1 - 2x2
         * 2 - 4x4
         * 3 - 8x8
         * 4 - 16x16
         * 5 - will uselesly consume your time, because max suported value is 4
         */
        /* threshold map (note: the patented pattern dithering algorithm uses 4x4) */
        static byte mapPow = 3;

        static ConcurrentDithering()
        {
            int mapSize = 1 << mapPow;
            candidatesCount = mapSize * mapSize;
            for (int i = 0; i < mapPow; i++)
            {
                mask <<= 1;
                mask |= 1;
            }

            map = new byte[mapSize, mapSize];


            for (uint x = 0; x < mapSize; x++)
            {
                uint invX = Invert(x, mapPow);

                for (uint y = 0; y < mapSize; y++)
                {
                    map[y, x] = (byte)(Interleave(invX, Invert(x ^ y, mapPow)));
                }
            }

            //----------
        }

        static byte[,] map;
        static int candidatesCount = 0;
        static byte mask = 0;



        /* Palette */
        static Color[] _pal;
        public static Color[] Pal
        {
            set
            {
                _pal = value;
                prepareCaches();
                BestMixingPlans.Clear();
            }
            get
            {
                return _pal;
            }
        }
        public static int[][] upal;


        static void prepareCaches()
        {
            upal = new int[_pal.Length][];
            luma = new int[_pal.Length];

            for (int i = 0, imax = _pal.Length; i < imax; i++)
            {
                int r = _pal[i].R, g = _pal[i].G, b = _pal[i].B;

                upal[i] = new int[] { r, g, b };
                luma[i] = rSensivityK * r + gSensivityK * g + bSensivityK * b;
            }
        }

        static int[] luma;

        const int rSensivityK = 299;
        const int gSensivityK = 587;
        const int bSensivityK = 114;

        static int ColorCompare(int palIndex, int r2, int g2, int b2) // Actually... This is algorithm for indexed pallete. We using nonindexed one, and pretty big (512 colors), by the way.
                                                                      // So, we can found an algebraic solution for this task and use it instead of brute force.
        {
            int[] pc = upal[palIndex];

            int luma1 = luma[palIndex];
            int luma2 = rSensivityK * r2 + gSensivityK * g2 + bSensivityK * b2;

            int lumadiff = (luma1 >> 3) - (luma2 >> 3);
            int diffR = pc[0] - r2, diffG = pc[1] - g2, diffB = pc[2] - b2;
            return ((diffR * diffR * rSensivityK + diffG * diffG * gSensivityK + diffB * diffB * bSensivityK) >> 5) * (700 >> 1)
                + (lumadiff * lumadiff);
        }
        class MixingPlan
        {
            public int[] colors; // pallete indexes
        };

        static Dictionary<Color, MixingPlan> BestMixingPlans = new Dictionary<Color, MixingPlan>();

        static MixingPlan DeviseBestMixingPlan(Color color)
        {
            MixingPlan result = new MixingPlan
            {
                colors = new int[candidatesCount]
            };

            int[] src = { color.R, color.G, color.B };

            const float X = 0.09f;  // Error multiplier
            int[] e = { 0, 0, 0 }; // Error accumulator
            for (int c = 0; c < result.colors.Length; ++c)
            {
                // Current temporary value
                int[] t = { (int)(src[0] + e[0] * X), (int)(src[1] + e[1] * X), (int)(src[2] + e[2] * X) };
                // Clamp it in the allowed RGB range
                if (t[0] < 0) t[0] = 0; else if (t[0] > 255) t[0] = 255;
                if (t[1] < 0) t[1] = 0; else if (t[1] > 255) t[1] = 255;
                if (t[2] < 0) t[2] = 0; else if (t[2] > 255) t[2] = 255;
                // Find the closest color from the palette
                int least_penalty = int.MaxValue;
                int chosen = c % upal.Length;
                for (int index = 0; index < upal.Length; ++index)
                {
                    int[] pc2 = upal[index];
                    int penalty = ColorCompare(index, t[0], t[1], t[2]);
                    if (penalty < least_penalty)
                    { least_penalty = penalty; chosen = index; }
                }
                // Add it to candidates and update the error
                result.colors[c] = chosen;
                int[] pc = upal[chosen];
                e[0] += src[0] - pc[0];
                e[1] += src[1] - pc[1];
                e[2] += src[2] - pc[2];
            }
            // Sort the colors according to luminance
            Array.Sort(result.colors, (i1, i2) => {
                return luma[i1].CompareTo(luma[i2]);
            });

            return result;
        }

        private static async Task<Dictionary<Color, MixingPlan>> DeviseBestMixingPlansAsync(List<Color> colors, Action<float> progressAction)
        {
            Dictionary<Color, MixingPlan> plans = new Dictionary<Color, MixingPlan>();

            await Task.Run(() =>
            {
                int progress = 0;
                foreach (var color in colors)
                {
                    progress++;
                    var plan = DeviseBestMixingPlan(color);
                    plans[color] = plan;
                    if (progress % 100 == 0)
                    {
                        progressAction.Invoke((float)progress/colors.Count);
                    } 
                }
            });

            return plans;
        }

        public static Action<float> onProgress = null;

        static int tasksCount = Environment.ProcessorCount; // all your CPU Time belongs to us!
        public static void DitherImage(Bitmap im) 
        {
            var test = new Stopwatch();
            test.Start();

            step = 0;
            tasksProgress = Enumerable.Range(0, tasksCount).Select(a => 0f).ToArray();
            opProgress = 0;

            int w = im.Width;
            int h = im.Height;
            UpdateProgress();
            HashSet<Color> usedColors = new HashSet<Color>();


            BitmapData data = im.LockBits(new Rectangle(Point.Empty, im.Size), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);

            Debug.Assert(sizeof(int) == 4); // Just in case...
            int lineWidth = data.Stride / 4;
            var pixelData = new int[lineWidth * data.Height];
            Marshal.Copy(data.Scan0, pixelData, 0, pixelData.Length);

            for (int y = 0; y < h; ++y)
            {
                for (int x = 0; x < w; ++x)
                {
                    Color color = Color.FromArgb(pixelData[lineWidth * y + x]);
                    usedColors.Add(color);
                }
                opProgress = (float)y / h;
                UpdateProgress();
            }

            step++;
            UpdateProgress();
            List<List<Color>> tasksData = new List<List<Color>>();

            for(int i = 0; i < tasksCount; i++)
            {
                tasksData.Add(new List<Color>());
            }

            int colorIndex = 0;
            int numSkiped = 0;
            foreach (var color in usedColors)
            {
                if (!BestMixingPlans.ContainsKey(color))
                {
                    tasksData[colorIndex % tasksCount].Add(color);
                    colorIndex++;
                }
                else
                {
                    numSkiped++;
                }
            }
            int taskIndex = 0;
            var tasks = tasksData.Select(d => {
                var lockedIndex = taskIndex;
                var task = DeviseBestMixingPlansAsync(d, p =>
                {
                    UpdateProgressForTask(lockedIndex, p);
                });
                taskIndex++;
                return task;
            }).ToArray();
            Task.WaitAll(tasks);
            step++;
            opProgress = 0;
            BestMixingPlans = tasks.SelectMany(t => t.Result.AsEnumerable()).Concat(BestMixingPlans.AsEnumerable()).ToDictionary(kvp => kvp.Key, kvp => kvp.Value);
            for (int y = 0; y < h; ++y)
            {
                for (int x = 0; x < w; ++x)
                {
                    Color color = Color.FromArgb(pixelData[lineWidth * y + x]);
                    byte map_value = map[x & mask, y & mask];
                    pixelData[lineWidth * y + x] = _pal[BestMixingPlans[color].colors[map_value]].ToArgb();
                }
                opProgress = (float)y / h;
                UpdateProgress();
            }

            Marshal.Copy(pixelData, 0, data.Scan0, pixelData.Length);
            im.UnlockBits(data);

            step++;
            UpdateProgress();

            test.Stop();
            //Console.WriteLine(string.Format("dithering time: {0}", test.ElapsedMilliseconds / 1000f));

        }

        static int step = 0;
        static float opProgress = 0;
        static float[] tasksProgress;
        private static void UpdateProgressForTask(int taskkIndex, float progress)
        {
            tasksProgress[taskkIndex] = progress;
            UpdateProgress();
        }

        static void UpdateProgress()
        {
            if (onProgress == null)
                return;

            float progressValue = 0;
            switch (step)
            {
                case 0:
                    progressValue = opProgress * 0.05f;
                    break;
                case 1:
                    progressValue = 0.05f + tasksProgress.Aggregate(0f, (a, b) => a + b) * 0.9f / tasksCount;
                    break;
                case 2:
                    progressValue = 0.95f + opProgress * 0.05f;
                    break;
                case 3:
                    progressValue = 1;
                    break;
            }

            onProgress(progressValue);
        }
    }

}