From 875cc0dae3a27e90d10b57d0752c83d258d64466 Mon Sep 17 00:00:00 2001
From: Mitch Souders <mitch.souders@gmail.com>
Date: Mon, 23 Aug 2021 22:31:15 -0700
Subject: [PATCH] Update color options, format

---
 src/color.rs         | 63 +++++++++++++++++++++++++++++++++++++++++++-
 src/epd5in65f/mod.rs | 12 +++++++--
 2 files changed, 72 insertions(+), 3 deletions(-)

diff --git a/src/color.rs b/src/color.rs
index 8867978..909bb10 100644
--- a/src/color.rs
+++ b/src/color.rs
@@ -72,9 +72,70 @@ impl From<()> for OctColor {
     }
 }
 
+#[cfg(feature = "graphics")]
+impl From<BinaryColor> for OctColor {
+    fn from(b: BinaryColor) -> OctColor {
+        match b {
+            BinaryColor::On => OctColor::Black,
+            BinaryColor::Off => OctColor::White,
+        }
+    }
+}
+
+#[cfg(feature = "graphics")]
+impl From<OctColor> for embedded_graphics::pixelcolor::Rgb888 {
+    fn from(b: OctColor) -> Self {
+        let (r, b, g) = b.rgb();
+        Self::new(r, b, g)
+    }
+}
+
+#[cfg(feature = "graphics")]
+impl From<embedded_graphics::pixelcolor::Rgb888> for OctColor {
+    fn from(p: embedded_graphics::pixelcolor::Rgb888) -> OctColor {
+        use embedded_graphics::prelude::RgbColor;
+        let colors = [
+            OctColor::Black,
+            OctColor::White,
+            OctColor::Green,
+            OctColor::Blue,
+            OctColor::Red,
+            OctColor::Yellow,
+            OctColor::Orange,
+            OctColor::HiZ,
+        ];
+        // if the user has already mapped to the right color space, it will just be in the list
+        if let Some(found) = colors.iter().find(|c| c.rgb() == (p.r(), p.g(), p.b())) {
+            return *found;
+        }
+
+        // This is not ideal but just pick the nearest color
+        *colors
+            .iter()
+            .map(|c| (c, c.rgb()))
+            .map(|(c, (r, g, b))| {
+                let dist = ((r - p.r()) as u32).pow(2)
+                    + ((g - p.g()) as u32).pow(2)
+                    + ((b - p.b()) as u32).pow(2);
+                (c, dist)
+            })
+            .min_by_key(|(_c, dist)| *dist)
+            .map(|(c, _)| c)
+            .unwrap_or(&OctColor::White)
+    }
+}
+
+#[cfg(feature = "graphics")]
+impl From<embedded_graphics::pixelcolor::raw::RawU4> for OctColor {
+    fn from(b: embedded_graphics::pixelcolor::raw::RawU4) -> Self {
+        use embedded_graphics::prelude::RawData;
+        OctColor::from_nibble(b.into_inner()).unwrap()
+    }
+}
+
 #[cfg(feature = "graphics")]
 impl PixelColor for OctColor {
-    type Raw = ();
+    type Raw = embedded_graphics::pixelcolor::raw::RawU4;
 }
 
 impl OctColor {
diff --git a/src/epd5in65f/mod.rs b/src/epd5in65f/mod.rs
index 869fe2a..3199e38 100644
--- a/src/epd5in65f/mod.rs
+++ b/src/epd5in65f/mod.rs
@@ -60,7 +60,7 @@ where
         self.cmd_with_data(spi, Command::BoosterSoftStart, &[0xC7, 0xC7, 0x1D])?;
         self.cmd_with_data(spi, Command::PllControl, &[0x3C])?;
         self.cmd_with_data(spi, Command::TemperatureSensor, &[0x00])?;
-        self.cmd_with_data(spi, Command::VcomAndDataIntervalSetting, &[0x37])?;
+        self.update_vcom(spi)?;
         self.cmd_with_data(spi, Command::TconSetting, &[0x22])?;
         self.send_resolution(spi)?;
 
@@ -68,7 +68,7 @@ where
 
         delay.delay_ms(100);
 
-        self.cmd_with_data(spi, Command::VcomAndDataIntervalSetting, &[0x37])?;
+        self.update_vcom(spi)?;
         Ok(())
     }
 }
@@ -118,6 +118,7 @@ where
         _delay: &mut DELAY,
     ) -> Result<(), SPI::Error> {
         self.wait_busy_high();
+        self.update_vcom(spi)?;
         self.send_resolution(spi)?;
         self.cmd_with_data(spi, Command::DataStartTransmission1, buffer)?;
         Ok(())
@@ -160,6 +161,7 @@ where
     fn clear_frame(&mut self, spi: &mut SPI, delay: &mut DELAY) -> Result<(), SPI::Error> {
         let bg = OctColor::colors_byte(self.color, self.color);
         self.wait_busy_high();
+        self.update_vcom(spi)?;
         self.send_resolution(spi)?;
         self.command(spi, Command::DataStartTransmission1)?;
         self.interface.data_x_times(spi, bg, WIDTH * HEIGHT / 2)?;
@@ -238,6 +240,12 @@ where
         self.send_data(spi, &[(h >> 8) as u8])?;
         self.send_data(spi, &[h as u8])
     }
+
+    fn update_vcom(&mut self, spi: &mut SPI) -> Result<(), SPI::Error> {
+        let bg_color = (self.color.get_nibble() & 0b111) << 5;
+        self.cmd_with_data(spi, Command::VcomAndDataIntervalSetting, &[0x17 | bg_color])?;
+        Ok(())
+    }
 }
 
 #[cfg(test)]