From 9f25ff3e2c54c95390aa5ce914aa80605fb7a497 Mon Sep 17 00:00:00 2001
From: Jeff Epler <jepler@gmail.com>
Date: Fri, 29 Oct 2021 14:20:21 -0500
Subject: [PATCH] WIP

---
 shared-module/bitmaptools/__init__.c |  8 +++-
 shared-module/displayio/Bitmap.c     | 65 ++++++++++++++++++++++++++++
 shared-module/displayio/Bitmap.h     |  1 +
 3 files changed, 72 insertions(+), 2 deletions(-)

diff --git a/shared-module/bitmaptools/__init__.c b/shared-module/bitmaptools/__init__.c
index 2e37a4651bd2..33a6d74e3f02 100644
--- a/shared-module/bitmaptools/__init__.c
+++ b/shared-module/bitmaptools/__init__.c
@@ -673,6 +673,7 @@ void common_hal_bitmaptools_dither(displayio_bitmap_t *dest_bitmap, displayio_bi
     int32_t row2[width];
     int32_t row3[width];
     int32_t *rows[3] = {row1, row2, row3};
+    uint32_t out[width];
 
     fill_row(source_bitmap, pixel_shader, rows[0], 0);
     fill_row(source_bitmap, pixel_shader, rows[1], 1);
@@ -684,7 +685,8 @@ void common_hal_bitmaptools_dither(displayio_bitmap_t *dest_bitmap, displayio_bi
         for (int x = 0; x < width; x++) {
             int32_t pixel_in = rows[0][x];
             bool pixel_out = pixel_in >= 128;
-            displayio_bitmap_write_pixel(dest_bitmap, x, y, pixel_out ? max_pixel : 0);
+            out[x] = pixel_out ? max_pixel : 0;
+
             int err = pixel_in - (pixel_out ? 255 : 0);
 
             for (int i = 0; i < info->count; i++) {
@@ -697,6 +699,7 @@ void common_hal_bitmaptools_dither(displayio_bitmap_t *dest_bitmap, displayio_bi
                 rows[dy][x1] = ((info->terms[i].dl * err) >> 8) + rows[dy][x1];
             }
         }
+        displayio_bitmap_write_pixels(dest_bitmap, out, 0, y, width);
 
         int32_t *tmp = rows[0];
         rows[0] = rows[1];
@@ -713,7 +716,7 @@ void common_hal_bitmaptools_dither(displayio_bitmap_t *dest_bitmap, displayio_bi
         for (int x = width; x--;) {
             int32_t pixel_in = rows[0][x];
             bool pixel_out = pixel_in >= 128;
-            displayio_bitmap_write_pixel(dest_bitmap, x, y, pixel_out ? max_pixel : 0);
+            out[x] = pixel_out ? max_pixel : 0;
             int err = pixel_in - (pixel_out ? 255 : 0);
 
             for (int i = 0; i < info->count; i++) {
@@ -726,6 +729,7 @@ void common_hal_bitmaptools_dither(displayio_bitmap_t *dest_bitmap, displayio_bi
                 rows[dy][x1] = ((info->terms[i].dl * err) >> 8) + rows[dy][x1];
             }
         }
+        displayio_bitmap_write_pixels(dest_bitmap, out, 0, y, width);
 
         tmp = rows[0];
         rows[0] = rows[1];
diff --git a/shared-module/displayio/Bitmap.c b/shared-module/displayio/Bitmap.c
index c3fb12de325a..ccc525d5a4f1 100644
--- a/shared-module/displayio/Bitmap.c
+++ b/shared-module/displayio/Bitmap.c
@@ -173,6 +173,71 @@ void common_hal_displayio_bitmap_get_pixels(displayio_bitmap_t *self, uint32_t *
 #undef BIG_PIXEL_LOOP
 }
 
+void displayio_bitmap_write_pixels(displayio_bitmap_t *self, const uint32_t *pixels, int16_t x, int16_t y, size_t n) {
+    // Writes the pixels directly to the bitmap
+    // Must update the dirty area separately
+    switch (self->bits_per_value) {
+
+#define SMALL_PIXEL_LOOP(sz) do { \
+        size_t *data = &self->data[y * self->stride + x / SIZE_BIT / sz]; \
+        /* First, put some initial pixels from an incompletely-used bytes */ \
+        while (x % SIZE_BIT / sz) { \
+            displayio_bitmap_write_pixel(self, x++, y, *pixels++); \
+            n--; \
+        } \
+        while (n > SIZE_BIT / sz) { \
+            size_t d = 0; \
+            for (size_t i = 0; i < SIZE_BIT; i++) { \
+                d <<= sz; \
+                d |= (*pixels++ & ((1 << sz) - 1)); \
+            } \
+            *data++ = d; \
+        } \
+        /* Put any straggling pixels */ \
+        while (n--) { \
+            displayio_bitmap_write_pixel(self, x++, y, *pixels++); \
+        } \
+} while (0)
+        case 1:
+            SMALL_PIXEL_LOOP(1);
+            break;
+
+        case 2:
+            SMALL_PIXEL_LOOP(2);
+            break;
+
+        case 4:
+            SMALL_PIXEL_LOOP(4);
+            break;
+
+#undef SMALL_PIXEL_LOOP
+
+#define BIG_PIXEL_LOOP(type) do { \
+        type *data = ((type *)&self->data[y * self->stride]) + x; \
+        while (n--) { \
+            *data++ = *pixels++; \
+        } \
+} while (0)
+
+        case 8:
+            BIG_PIXEL_LOOP(uint8_t);
+            break;
+
+        case 16:
+            BIG_PIXEL_LOOP(uint16_t);
+            break;
+
+        case 32:
+            BIG_PIXEL_LOOP(uint32_t);
+            break;
+
+        default:
+            abort();
+    }
+
+#undef BIG_PIXEL_LOOP
+}
+
 void displayio_bitmap_set_dirty_area(displayio_bitmap_t *self, const displayio_area_t *dirty_area) {
     if (self->read_only) {
         mp_raise_RuntimeError(translate("Read-only object"));
diff --git a/shared-module/displayio/Bitmap.h b/shared-module/displayio/Bitmap.h
index 82a3de631a20..92fa0e0bd96c 100644
--- a/shared-module/displayio/Bitmap.h
+++ b/shared-module/displayio/Bitmap.h
@@ -51,5 +51,6 @@ void displayio_bitmap_finish_refresh(displayio_bitmap_t *self);
 displayio_area_t *displayio_bitmap_get_refresh_areas(displayio_bitmap_t *self, displayio_area_t *tail);
 void displayio_bitmap_set_dirty_area(displayio_bitmap_t *self, const displayio_area_t *area);
 void displayio_bitmap_write_pixel(displayio_bitmap_t *self, int16_t x, int16_t y, uint32_t value);
+void displayio_bitmap_write_pixels(displayio_bitmap_t *self, const uint32_t *pixels, int16_t x, int16_t y, size_t n);
 
 #endif // MICROPY_INCLUDED_SHARED_MODULE_DISPLAYIO_BITMAP_H