diff --git a/src/display.rs b/src/display.rs
new file mode 100644
index 0000000..51f543c
--- /dev/null
+++ b/src/display.rs
@@ -0,0 +1,237 @@
+use atmega_hal::{
+    Spi,
+    port::{self, Pin, PinOps, mode},
+    spi::ChipSelectPin,
+};
+use embedded_hal::{delay::DelayNs, digital::OutputPin, spi::SpiBus};
+
+pub struct Rgb565(pub u8, pub u8, pub u8);
+
+impl Rgb565 {
+    pub fn as_color(&self) -> Color {
+        let r = ((self.0 / 8) as u16) << 11;
+        let g = ((self.1 / 4) as u16) << 5;
+        let b = (self.2 / 8) as u16;
+
+        Color::from(r | b | g)
+    }
+}
+
+#[derive(Default, Clone, Copy)]
+pub struct Position {
+    pub x: u16,
+    pub y: u16,
+}
+
+#[derive(Default, Clone, Copy)]
+pub struct Color {
+    pub bytes: [u8; 2],
+}
+
+impl Color {
+    pub fn from(number: u16) -> Color {
+        Color {
+            bytes: number.to_be_bytes(),
+        }
+    }
+}
+
+pub struct Display<T: DelayNs, DCPin: PinOps, RSTPin: PinOps> {
+    pub spi: Spi,
+    pub cs: ChipSelectPin<port::PB2>,
+    pub dc: Pin<mode::Output, DCPin>,
+    pub rst: Pin<mode::Output, RSTPin>,
+    pub delay: T,
+}
+
+enum Command {
+    NOP = 0x0,
+    SoftReset = 0x1,
+    RddID = 0x4,
+    RddST = 0x9,
+
+    SleepIn = 0x10,
+    SleepOut = 0x11,
+    PTLON = 0x12,
+    NoRon = 0x13,
+
+    InversionOff = 0x20,
+    InversionOn = 0x21,
+    DisplayOff = 0x28,
+    DisplayOn = 0x29,
+    ColumnAlignmentSet = 0x2A,
+    RowAlignmentSet = 0x2B,
+    RamWR = 0x2C,
+    RamRD = 0x2E,
+
+    PTLAR = 0x30,
+    VSCRDEF = 0x33,
+    ColorMode = 0x3A,
+    MADCTL = 0x36,
+    VSCSAD = 0x37,
+    // MadCTLMY = 0x80,
+    // MadCTLMX = 0x40,
+    // MadCTLMV = 0x20,
+    // MadCTLML = 0x10,
+    // MadCTLBGR = 0x08,
+    // MadCTLMH = 0x04,
+    // MadCTLRGB = 0x00,
+
+    // RDID1 = 0xDA,
+    // RDID2 = 0xDB,
+    // RDID3 = 0xDC,
+    // RDID4 = 0xDD,
+
+    // ColorMode65K = 0x50,
+    // ColorMode262K = 0x60,
+    // ColorMode12BIT = 0x03,
+    // ColorMode16BIT = 0x05,
+    // ColorMode18BIT = 0x06,
+    // ColorMode16M = 0x07,
+}
+
+enum ColorMode {
+    ColorMode65K = 0x50,
+    ColorMode262K = 0x60,
+    ColorMode12BIT = 0x03,
+    ColorMode16BIT = 0x05,
+    ColorMode18BIT = 0x06,
+    ColorMode16M = 0x07,
+}
+
+enum Writeable<'d> {
+    Command(Command),
+    Data(&'d [u8]),
+}
+
+enum Rotation {
+    Portrait = 0x00,
+    Landscape = 0x60,
+    InvertedPortrait = 0xc0,
+    InvertedLandscape = 0xa0,
+}
+
+impl<T: DelayNs, DCPin: PinOps, RSTPin: PinOps> Display<T, DCPin, RSTPin> {
+    fn write(&mut self, writeable: Writeable) {
+        self.cs.set_low().unwrap();
+        match writeable {
+            Writeable::Command(cmd) => {
+                self.dc.set_low();
+                SpiBus::write(&mut self.spi, &[cmd as u8]).unwrap()
+            }
+            Writeable::Data(data) => {
+                self.dc.set_high();
+                SpiBus::write(&mut self.spi, data).unwrap();
+                self.cs.set_high().unwrap();
+            }
+        }
+    }
+
+    pub fn init(&mut self) {
+        self.hard_reset();
+        self.soft_reset();
+        self.set_sleep(false);
+        self.set_color_mode((ColorMode::ColorMode65K as u8) | (ColorMode::ColorMode16BIT as u8));
+        self.delay.delay_ms(50);
+        self.set_inversion(true);
+        self.write(Writeable::Command(Command::NoRon));
+        self.draw_rect(
+            Position { x: 0, y: 0 },
+            Position { x: 240, y: 240 },
+            Color::default(),
+        );
+
+        self.write(Writeable::Command(Command::DisplayOn));
+
+        self.delay.delay_ms(500);
+    }
+
+    pub fn hard_reset(&mut self) {
+        self.cs.set_low().unwrap();
+        self.rst.set_high();
+        self.delay.delay_ms(50);
+
+        self.rst.set_low();
+        self.delay.delay_ms(50);
+
+        self.rst.set_high();
+        self.delay.delay_ms(150);
+
+        self.cs.set_high().unwrap();
+    }
+
+    pub fn soft_reset(&mut self) {
+        self.write(Writeable::Command(Command::SoftReset));
+        self.delay.delay_ms(150);
+    }
+
+    pub fn set_sleep(&mut self, sleep: bool) {
+        match sleep {
+            true => self.write(Writeable::Command(Command::SleepIn)),
+            false => self.write(Writeable::Command(Command::SleepOut)),
+        }
+    }
+
+    pub fn set_inversion(&mut self, inversion: bool) {
+        match inversion {
+            true => self.write(Writeable::Command(Command::InversionOn)),
+            false => self.write(Writeable::Command(Command::InversionOff)),
+        }
+    }
+
+    pub fn set_rotation(&mut self, rotation: Rotation) {
+        self.write(Writeable::Command(Command::MADCTL));
+        self.write(Writeable::Data(&[rotation as u8]));
+    }
+
+    pub fn set_color_mode(&mut self, mode: u8) {
+        self.write(Writeable::Command(Command::ColorMode));
+        self.write(Writeable::Data(&[mode & 0x77]));
+    }
+
+    fn set_columns(&mut self, start: u16, end: u16) {
+        let [start1, start2] = start.to_be_bytes();
+        let [end1, end2] = end.to_be_bytes();
+        self.write(Writeable::Command(Command::ColumnAlignmentSet));
+        self.write(Writeable::Data(&[start1, start2, end1, end2]));
+    }
+
+    fn set_rows(&mut self, start: u16, end: u16) {
+        let [start1, start2] = start.to_be_bytes();
+        let [end1, end2] = end.to_be_bytes();
+        self.write(Writeable::Command(Command::RowAlignmentSet));
+        self.write(Writeable::Data(&[start1, start2, end1, end2]));
+    }
+
+    fn set_window(&mut self, pos0: Position, pos1: Position) {
+        self.set_columns(pos0.x, pos1.x);
+        self.set_rows(pos0.y, pos1.y);
+        self.write(Writeable::Command(Command::RamWR));
+    }
+
+    pub fn pixel(&mut self, position: Position, color: Color) {
+        self.set_window(position, position);
+        self.write(Writeable::Data(&color.bytes));
+    }
+
+    pub fn draw_rect(&mut self, pos0: Position, pos1: Position, color: Color) {
+        self.set_window(pos0, pos1);
+        self.dc.set_high();
+        let width = pos1.x - pos0.x;
+        let height = pos1.y - pos0.y;
+        let pixels = width * height;
+        let chunks = pixels / 256;
+        let mut full_buf = [0; 512];
+        let [col1, col2] = color.bytes;
+        for i in 0..256 {
+            full_buf[i * 2] = col1;
+            full_buf[i * 2 + 1] = col2;
+        }
+        for _ in 0..chunks {
+            self.write(Writeable::Data(&full_buf));
+        }
+        for _ in 0..(pixels % 256) {
+            self.write(Writeable::Data(&color.bytes));
+        }
+    }
+}
diff --git a/src/main.rs b/src/main.rs
index cd4d230..1152a93 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -17,6 +17,10 @@ use embedded_hal::spi::{Operation, SpiBus};
 use embedded_hal::{delay::DelayNs, digital::OutputPin};
 use panic_halt as _;
 
+use crate::display::{Color, Display, Position, Rgb565};
+
+mod display;
+
 type CoreClock = atmega_hal::clock::MHz8;
 
 struct Device<T: DelayNs> {
@@ -56,212 +60,6 @@ impl<T: DelayNs> embedded_hal::spi::SpiDevice for Device<T> {
     }
 }
 
-#[derive(Default, Clone, Copy)]
-struct Position {
-    x: u16,
-    y: u16,
-}
-
-impl Position {
-    fn as_bytes(&self) -> [u8; 4] {
-        let [x1, x2] = self.x.to_be_bytes();
-        let [y1, y2] = self.y.to_be_bytes();
-        [x1, x2, y1, y2]
-    }
-}
-
-#[derive(Default, Clone, Copy)]
-struct Color {
-    color: u16,
-}
-
-impl Color {
-    fn as_bytes(&self) -> [u8; 2] {
-        self.color.to_be_bytes()
-    }
-}
-
-struct Display<T: DelayNs, DCPin: PinOps, RSTPin: PinOps> {
-    spi: Spi,
-    cs: ChipSelectPin<port::PB2>,
-    dc: Pin<mode::Output, DCPin>,
-    rst: Pin<mode::Output, RSTPin>,
-    delay: T,
-}
-
-enum Command {
-    NOP = 0x0,
-    SoftReset = 0x1,
-    RddID = 0x4,
-    RddST = 0x9,
-
-    SleepIn = 0x10,
-    SleepOut = 0x11,
-    PTLON = 0x12,
-    NoRon = 0x13,
-
-    InversionOff = 0x20,
-    InversionOn = 0x21,
-    DisplayOff = 0x28,
-    DisplayOn = 0x29,
-    ColumnAlignmentSet = 0x2A,
-    RowAlignmentSet = 0x2B,
-    RamWR = 0x2C,
-    RamRD = 0x2E,
-
-    PTLAR = 0x30,
-    VSCRDEF = 0x33,
-    ColorMode = 0x3A,
-    MADCTL = 0x36,
-    VSCSAD = 0x37,
-    // MadCTLMY = 0x80,
-    // MadCTLMX = 0x40,
-    // MadCTLMV = 0x20,
-    // MadCTLML = 0x10,
-    // MadCTLBGR = 0x08,
-    // MadCTLMH = 0x04,
-    // MadCTLRGB = 0x00,
-
-    // RDID1 = 0xDA,
-    // RDID2 = 0xDB,
-    // RDID3 = 0xDC,
-    // RDID4 = 0xDD,
-
-    // ColorMode65K = 0x50,
-    // ColorMode262K = 0x60,
-    // ColorMode12BIT = 0x03,
-    // ColorMode16BIT = 0x05,
-    // ColorMode18BIT = 0x06,
-    // ColorMode16M = 0x07,
-}
-
-enum ColorMode {
-    ColorMode65K = 0x50,
-    ColorMode262K = 0x60,
-    ColorMode12BIT = 0x03,
-    ColorMode16BIT = 0x05,
-    ColorMode18BIT = 0x06,
-    ColorMode16M = 0x07,
-}
-
-enum Writeable<'d> {
-    Command(Command),
-    Data(&'d [u8]),
-}
-
-enum Rotation {
-    Portrait = 0x00,
-    Landscape = 0x60,
-    InvertedPortrait = 0xc0,
-    InvertedLandscape = 0xa0,
-}
-
-impl<T: DelayNs, DCPin: PinOps, RSTPin: PinOps> Display<T, DCPin, RSTPin> {
-    fn write(&mut self, writeable: Writeable) {
-        self.cs.set_low().unwrap();
-        match writeable {
-            Writeable::Command(cmd) => {
-                self.dc.set_low();
-                SpiBus::write(&mut self.spi, &[cmd as u8]).unwrap()
-            }
-            Writeable::Data(data) => {
-                self.dc.set_high();
-                SpiBus::write(&mut self.spi, data).unwrap();
-                self.cs.set_high().unwrap();
-            }
-        }
-    }
-
-    fn hard_reset(&mut self) {
-        self.cs.set_low().unwrap();
-        self.rst.set_high();
-        self.delay.delay_ms(50);
-
-        self.rst.set_low();
-        self.delay.delay_ms(50);
-
-        self.rst.set_high();
-        self.delay.delay_ms(150);
-
-        self.cs.set_high().unwrap();
-    }
-
-    fn soft_reset(&mut self) {
-        self.write(Writeable::Command(Command::SoftReset));
-        self.delay.delay_ms(150);
-    }
-
-    fn set_sleep(&mut self, sleep: bool) {
-        match sleep {
-            true => self.write(Writeable::Command(Command::SleepIn)),
-            false => self.write(Writeable::Command(Command::SleepOut)),
-        }
-    }
-
-    fn set_inversion(&mut self, inversion: bool) {
-        match inversion {
-            true => self.write(Writeable::Command(Command::InversionOn)),
-            false => self.write(Writeable::Command(Command::InversionOff)),
-        }
-    }
-
-    fn set_rotation(&mut self, rotation: Rotation) {
-        self.write(Writeable::Command(Command::MADCTL));
-        self.write(Writeable::Data(&[rotation as u8]));
-    }
-
-    fn set_color_mode(&mut self, mode: u8) {
-        self.write(Writeable::Command(Command::ColorMode));
-        self.write(Writeable::Data(&[mode & 0x77]));
-    }
-
-    fn set_columns(&mut self, start: u16, end: u16) {
-        let [start1, start2] = start.to_be_bytes();
-        let [end1, end2] = end.to_be_bytes();
-        self.write(Writeable::Command(Command::ColumnAlignmentSet));
-        self.write(Writeable::Data(&[start1, start2, end1, end2]));
-    }
-
-    fn set_rows(&mut self, start: u16, end: u16) {
-        let [start1, start2] = start.to_be_bytes();
-        let [end1, end2] = end.to_be_bytes();
-        self.write(Writeable::Command(Command::RowAlignmentSet));
-        self.write(Writeable::Data(&[start1, start2, end1, end2]));
-    }
-
-    fn set_window(&mut self, pos0: Position, pos1: Position) {
-        self.set_columns(pos0.x, pos1.x);
-        self.set_rows(pos0.y, pos1.y);
-        self.write(Writeable::Command(Command::RamWR));
-    }
-
-    fn pixel(&mut self, position: Position, color: Color) {
-        self.set_window(position, position);
-        self.write(Writeable::Data(&color.as_bytes()));
-    }
-
-    fn draw_rect(&mut self, pos0: Position, pos1: Position, color: Color) {
-        self.set_window(pos0, pos1);
-        self.dc.set_high();
-        let width = pos1.x - pos0.x;
-        let height = pos1.y - pos0.y;
-        let pixels = width * height;
-        let chunks = pixels / 256;
-        let mut full_buf = [0; 512];
-        let [col1, col2] = color.as_bytes();
-        for i in 0..256 {
-            full_buf[i * 2] = col1;
-            full_buf[i * 2 + 1] = col2;
-        }
-        for _ in 0..chunks {
-            self.write(Writeable::Data(&full_buf));
-        }
-        for _ in 0..(pixels % 256) {
-            self.write(Writeable::Data(&color.as_bytes()));
-        }
-    }
-}
-
 #[avr_device::entry]
 fn main() -> ! {
     let dp = atmega_hal::Peripherals::take().unwrap();
@@ -302,27 +100,19 @@ fn main() -> ! {
         delay: atmega_hal::delay::Delay::<CoreClock>::new(),
     };
 
-    display.hard_reset();
-    display.soft_reset();
-    display.set_sleep(false);
-    display.set_color_mode((ColorMode::ColorMode65K as u8) | (ColorMode::ColorMode16BIT as u8));
-    display.delay.delay_ms(50);
-    display.set_inversion(false);
-    display.write(Writeable::Command(Command::NoRon));
-    display.pixel(Position::default(), Color::default());
-
-    display.write(Writeable::Command(Command::DisplayOn));
+    display.init();
 
     let mut delay = atmega_hal::delay::Delay::<CoreClock>::new();
-    delay.delay_ms(1000);
+    let mut color = 0;
     loop {
         // color += 40;
         delay.delay_ms(1000);
         display.draw_rect(
             Position { x: 0, y: 0 },
             Position { x: 200, y: 200 },
-            Color::default(),
+            Rgb565(0, 0, 255).as_color(),
         );
+        color = color + 5 % 255;
         ufmt::uwriteln!(&mut serial, "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA").unwrap();
     }
 }