Implement cammy_photo_dither()

include/cammy/photo.h

@@ -38,4 +38,10 @@ void cammy_photo_export(cammy_photo *src, uint8_t *dest, int stride);
 
 void cammy_photo_import(cammy_photo *dest, uint8_t *src, int stride);
 
+void cammy_photo_dither(uint8_t *dest,
+                        uint8_t *src,
+                            int  width,
+                            int  height,
+                            int  stride);
+
 #endif /* _CAMMY_PHOTO_H */

src/photo.c

@@ -134,3 +134,39 @@ void cammy_photo_import(cammy_photo *dest, uint8_t *src, int stride) {
         }
     }
 }
+
+void cammy_photo_dither(uint8_t *dest,
+                        uint8_t *src,
+                            int  width,
+                            int  height,
+                            int  stride) {
+    size_t x, y;
+
+    for (y=0; y<height; y++) {
+        for (x=0; x<width; x++) {
+            uint8_t r = src[stride*width*y+stride*x],
+                    g = src[stride*width*y+stride*x+1],
+                    b = src[stride*width*y+stride*x+2];
+
+            uint8_t gray = (uint8_t)
+                ((0.2126 * (float)r)
+               + (0.7152 * (float)g)
+               + (0.0722 * (float)b));
+
+            uint32_t slot = bayer_matrix[gray];
+            uint32_t from = (slot & 0x03000000) >> 24;
+            uint32_t to   = (slot & 0x00030000) >> 16;
+            uint8_t  value;
+
+            if (slot & (0x8000 >> ((y & 3) << 2) >> (x & 3))) {
+                value = to   ^ 0x03;
+            } else {
+                value = from ^ 0x03;
+            }
+
+            dest[stride*CAMMY_PHOTO_WIDTH*y+stride*x]   = value;
+            dest[stride*CAMMY_PHOTO_WIDTH*y+stride*x+1] = value;
+            dest[stride*CAMMY_PHOTO_WIDTH*y+stride*x+2] = value;
+        }
+    }
+}