From 180b36203bd6d509680bebf4bd079784be3b920e Mon Sep 17 00:00:00 2001
From: XANTRONIX Development <dev@xantronix.com>
Date: Sun, 15 May 2016 02:37:19 -0500
Subject: [PATCH] Implement cammy_photo_dither()

---
 include/cammy/photo.h |  6 ++++++
 src/photo.c           | 36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 42 insertions(+)

diff --git a/include/cammy/photo.h b/include/cammy/photo.h
index fcbaac2..89e2aea 100644
--- a/include/cammy/photo.h
+++ b/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 */
diff --git a/src/photo.c b/src/photo.c
index a5846ee..0ffa566 100644
--- a/src/photo.c
+++ b/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;
+        }
+    }
+}