1 files changed, 48 insertions, 4 deletions

diff --git a/src/ws2812b.c b/src/ws2812b.c
index 1dea9f1..692e8ad 100644
--- a/src/ws2812b.c
+++ b/src/ws2812b.c
@@ -2,6 +2,7 @@
 
 #include <string.h>
 
+#include "byte_math.h"
 #include "spi.h"
 #include "sysled.h"
 
@@ -38,9 +39,9 @@ inline static void complie_color_inline(
     color.g = tmp;
   }
 
-  color.r = color.r;
-  color.g = color.g;
-  color.b = color.b;
+  color.r = byte_scale(color.r, 255 - color.a);
+  color.g = byte_scale(color.g, 255 - color.a);
+  color.b = byte_scale(color.b, 255 - color.a);
 
   out->first_bits =
       BYTE_1(COLOR_BYTE_1(color.r)) | BYTE_2(COLOR_BYTE_2(color.r)) |
@@ -79,7 +80,8 @@ int write_rgb(struct ws2812b_buf* out, rgb_t color)
     while ((dma_loc - CLIP_TO_DMA_BITS(out->buf + out->cur) <
             TOTAL_BYTES_PER_LED) &&
            (dma_loc < dma_end)) {
-      set_sysled(1); // Little indication to tell if we are waiting for the DMA.
+      set_sysled(
+          1);  // Little indication to tell if we are waiting for the DMA.
       dma_loc = *dma_now;
     }
     set_sysled(0);
@@ -120,3 +122,45 @@ void compiled_color_dbg_str(struct rgb_compiled* c, char* out)
   }
   *(out++) = 0;
 }
+
+rgb_t blend(rgb_t c1, rgb_t c2)
+{
+  rgb_t r;
+
+  uint8_t prop = 255 - c1.a;
+  uint8_t rprop = 255 - prop;
+
+  r.r = clip((int)byte_scale(c1.r, prop) + byte_scale(c2.r, rprop));
+  r.g = clip((int)byte_scale(c1.g, prop) + byte_scale(c2.g, rprop));
+  r.b = clip((int)byte_scale(c1.b, prop) + byte_scale(c2.b, rprop));
+  r.a = 255 - (prop + byte_scale(255 - c2.a, rprop));
+
+  return r;
+}
+
+rgb_t mat_mul(rgb_t rgb, const rgb_mat_t* mat)
+{
+  rgb_t ret;
+
+  ret.r = byte_scale(rgb.r, mat->mat[0][0]) +
+          byte_scale(rgb.g, mat->mat[0][1]) +
+          byte_scale(rgb.b, mat->mat[0][2]) +
+          byte_scale(rgb.a, mat->mat[0][3]);
+
+  ret.g = byte_scale(rgb.r, mat->mat[1][0]) +
+          byte_scale(rgb.g, mat->mat[1][1]) +
+          byte_scale(rgb.b, mat->mat[1][2]) +
+          byte_scale(rgb.a, mat->mat[1][3]);
+
+  ret.b = byte_scale(rgb.r, mat->mat[2][0]) +
+          byte_scale(rgb.g, mat->mat[2][1]) +
+          byte_scale(rgb.b, mat->mat[2][2]) +
+          byte_scale(rgb.a, mat->mat[2][3]);
+
+  ret.a = byte_scale(rgb.r, mat->mat[3][0]) +
+          byte_scale(rgb.g, mat->mat[3][1]) +
+          byte_scale(rgb.b, mat->mat[3][2]) +
+          byte_scale(rgb.a, mat->mat[3][3]);
+
+  return ret;
+}