Implement trinary image encoding

eink.hpp

@@ -6,6 +6,7 @@
 
 #include "../clock.hpp"
 #include "../io/io.hpp"
+#include "../util/util.hpp"
 
 namespace eink {
 
@@ -109,16 +110,65 @@ class Eink {
 		_delay_ms(200);
 	}
 
-	static void draw(const uint8_t *blackFrame, const uint8_t *redFrame)
+	static void draw(const uint8_t *image)
 	{
-		sendCommand(Cmd::WRITE_RAM_BLACK);
-		for (auto i = uint16_t{0}; i < Width * Height / 8; i++) {
-			sendData(pgm_read_byte(&blackFrame[i]));
-		}
-		sendCommand(Cmd::WRITE_RAM_RED);
-		for (auto i = uint16_t{0}; i < Width * Height / 8; i++) {
-			sendData(~pgm_read_byte(&redFrame[i]));
-		}
+		constexpr auto BLOCK_SIZE = 5;
+
+		enum class Color : uint8_t {
+			BLACK = 0b00,
+			WHITE = 0b01,
+			RED = 0b10,
+			ERROR = 0b11,
+		};
+
+		class Block {
+		  public:
+			inline Color &operator[](const size_t idx)
+			{
+				return data[idx];
+			}
+
+			inline const Color &operator[](const size_t idx) const
+			{
+				return data[idx];
+			}
+
+		  private:
+			Color data[BLOCK_SIZE];
+		};
+
+		constexpr auto lookup = [](uint8_t bits) {
+			auto block = Block{};
+			for_constexpr(
+			    [&](const auto idx) {
+				    block[idx.value] = static_cast<Color>(bits % 3);
+				    bits /= 3;
+			    },
+			    util::make_index_sequence<BLOCK_SIZE>{});
+			return block;
+		};
+
+		constexpr auto sendImageChannel = [lookup](const auto command, const auto image) {
+			sendCommand(command);
+			auto buffer = uint8_t{0};
+			auto bufferPos = uint8_t{0};
+			for (auto i = uint16_t{0}; i < Width * Height / BLOCK_SIZE; i++) {
+				const auto block = lookup(pgm_read_byte(&image[i]));
+				for (auto p = uint8_t{0}; p < BLOCK_SIZE; ++p) {
+					const auto pixel = uint8_t{(command == Cmd::WRITE_RAM_BLACK) ? (block[p] != Color::BLACK)
+					                                                             : (block[p] == Color::RED)};
+					buffer |= pixel << (7 - bufferPos++);
+					if (bufferPos == 8) {
+						sendData(buffer);
+						buffer = 0;
+						bufferPos = 0;
+					}
+				}
+			}
+		};
+
+		sendImageChannel(Cmd::WRITE_RAM_BLACK, image);
+		sendImageChannel(Cmd::WRITE_RAM_RED, image);
 
 		sendCommand(Cmd::DISPLAY_UPDATE_CONTROL_2);
 		sendData(0xF7);