diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml
new file mode 100644
index 0000000..1852c6f
--- /dev/null
+++ b/.github/workflows/cmake.yml
@@ -0,0 +1,41 @@
+name: CMake
+
+on:
+  push:
+    branches: [ "main" ]
+  pull_request:
+    branches: [ "main" ]
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v4
+
+    - name: Install dependencies
+      run: |
+        sudo apt install software-properties-common
+        sudo add-apt-repository ppa:ubuntu-toolchain-r/test
+        sudo apt-get update
+        sudo apt install cmake gcc-13 g++-13
+
+        sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 60
+        sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 60
+        sudo update-alternatives --config gcc
+        sudo update-alternatives --config g++
+
+    - name: Linting
+      run: find -name "*.cc" -o  -name "*.c" -o  -name "*.hh" -o  -name "*.h" | xargs clang-format -n 
+
+    - name: Build 
+      run: |
+        cmake -B build -S ./ && cmake --build build
+
+    - name: Run st7735_driver_test 
+      run: |
+        ./build/tests/st7735_driver_test
+        
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..d928fe9
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.13)
+
+project(display_drivers LANGUAGES C CXX)
+
+# Set the C++ standard to C++20
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# For lsp
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+
+set(NAME st7735_driver)
+
+add_subdirectory(src)
+add_subdirectory(tests)
diff --git a/README.md b/README.md
index b2f794f..e9923a5 100644
--- a/README.md
+++ b/README.md
@@ -41,8 +41,8 @@ void main(void){
 
     lcd_st7735_init(&ctx, &interface);
     lcd_st7735_startup(&ctx);
-    lcd_st7735_fill_rectangle(&ctx, (LCD_rectangle){.origin = {.x = 0, .y = 0},
-        .end = {.x = 160, .y = 128}}, 0x00FF00);
+    lcd_st7735_fill_rectangle(
+       &ctx_, (LCD_rectangle){.origin = {.x = 0, .y = 0}, .width = 160, .height = 128}, 0x00FF00);
 }
 ```
 
@@ -86,3 +86,18 @@ void main(void){
 }
 ```
 
+## Running unittests
+Start nix development environment
+```sh
+nix develop
+```
+Build using Cmake
+```sh
+cmake -S . -B ./build/. && cmake --build ./build/.
+```
+Run the test
+```sh
+./build/tests/st7735_driver_test
+```
+
+
diff --git a/flake.lock b/flake.lock
new file mode 100644
index 0000000..1d6cf26
--- /dev/null
+++ b/flake.lock
@@ -0,0 +1,61 @@
+{
+  "nodes": {
+    "flake-utils": {
+      "inputs": {
+        "systems": "systems"
+      },
+      "locked": {
+        "lastModified": 1731533236,
+        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "type": "github"
+      }
+    },
+    "nixpkgs": {
+      "locked": {
+        "lastModified": 1744440957,
+        "narHash": "sha256-FHlSkNqFmPxPJvy+6fNLaNeWnF1lZSgqVCl/eWaJRc4=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "26d499fc9f1d567283d5d56fcf367edd815dba1d",
+        "type": "github"
+      },
+      "original": {
+        "owner": "NixOS",
+        "ref": "nixos-24.11",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
+    "root": {
+      "inputs": {
+        "flake-utils": "flake-utils",
+        "nixpkgs": "nixpkgs"
+      }
+    },
+    "systems": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
+    }
+  },
+  "root": "root",
+  "version": 7
+}
diff --git a/flake.nix b/flake.nix
new file mode 100644
index 0000000..6aee2f6
--- /dev/null
+++ b/flake.nix
@@ -0,0 +1,28 @@
+{
+    description = "C/C++ environment";
+
+    inputs = {
+         nixpkgs.url = "github:NixOS/nixpkgs/nixos-24.11";
+         flake-utils.url = "github:numtide/flake-utils";
+    };
+
+outputs = { self, nixpkgs, flake-utils, ... }:
+    flake-utils.lib.eachDefaultSystem (system:
+      let
+        pkgs = import nixpkgs { inherit system; };
+      in
+      {
+        devShells.default = pkgs.mkShell {
+          buildInputs = with pkgs; [
+            cmake
+            clang-tools_18
+            gdb
+            python310
+          ];
+          shellHook = ''
+            echo "C++ dev environment ready!"
+          '';
+        };
+      }
+    );
+}
diff --git a/simulator/st7735/controller.hh b/simulator/st7735/controller.hh
new file mode 100644
index 0000000..5c44815
--- /dev/null
+++ b/simulator/st7735/controller.hh
@@ -0,0 +1,238 @@
+
+#include <src/st7735/lcd_st7735.h>
+
+#include <array>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <format>
+#include <fstream>
+#include <string>
+
+#define STB_IMAGE_WRITE_IMPLEMENTATION
+#include "stb_image_write.h"
+
+#ifdef SIMULATOR_LOGGING
+#include <iostream>
+#define LOG(msg) std::cout << msg
+#else
+#define LOG(msg)                                                                                                       \
+  do {                                                                                                                 \
+  } while (0)
+#endif
+
+namespace Simulator {
+
+// Forwward declaration
+template <size_t width, size_t height>
+class St7735;
+
+template <size_t width, size_t height>
+class State {
+ public:
+  virtual ~State()                                                              = default;
+  virtual void handle(St7735<width, height>& sim, std::vector<uint8_t>& buffer) = 0;
+};
+
+template <size_t width, size_t height>
+class CommandState : public State<width, height> {
+ public:
+  void handle(St7735<width, height>& sim, std::vector<uint8_t>& buffer) override { sim.parse_commands(buffer); }
+};
+
+template <size_t width, size_t height>
+class CasetState : public State<width, height> {
+ public:
+  void handle(St7735<width, height>& sim, std::vector<uint8_t>& buffer) override { sim.parse_caset(buffer); }
+};
+
+template <size_t width, size_t height>
+class RasetState : public State<width, height> {
+ public:
+  void handle(St7735<width, height>& sim, std::vector<uint8_t>& buffer) override { sim.parse_raset(buffer); }
+};
+
+template <size_t width, size_t height>
+class RamWriteState : public State<width, height> {
+ public:
+  void handle(St7735<width, height>& sim, std::vector<uint8_t>& buffer) override { sim.ram_write(buffer); }
+};
+
+enum class PinLevel {
+  Low  = 0,
+  High = 1,
+};
+
+union __attribute__((packed)) Pixel {
+  struct {
+    uint8_t r;
+    uint8_t g;
+    uint8_t b;
+  } rgb;
+  uint8_t buffer[3];
+};
+
+struct Cursor {
+  size_t col_start, col_end, row_start, row_end, row, col;
+  void operator++(int) {
+    if (++col > col_end) {
+      col = col_start;
+      if (row++ > row_end) {
+        row = row_start;
+      }
+    }
+  }
+};
+
+template <size_t width, size_t height>
+class St7735 {
+  State<width, height>* state = new CommandState<width, height>();
+
+  std::array<std::array<Pixel, width>, height> frame_buffer;
+  Cursor cursor;
+
+  PinLevel dc_pin_ = PinLevel::High;
+  PinLevel cs_pin_ = PinLevel::High;
+  LCD_Orientation orientation_;
+
+ public:
+  St7735() {}
+
+  void set_state(State<width, height>* new_state) { state = new_state; }
+  void update(std::vector<uint8_t>& data) { state->handle(*this, data); }
+
+  void spi_write(uint8_t* data, size_t len) {
+    std::vector<uint8_t> vec(data, data + len);
+    update(vec);
+  }
+
+  void parse_commands(std::vector<uint8_t>& buffer) {
+    LOG(std::format("{}: ", __func__));
+    uint8_t cmd = buffer[0];
+    switch (cmd) {
+      break;
+      case ST7735_CASET:
+        LOG(std::format("CASET: "));
+        this->set_state(new CasetState<width, height>());
+        break;
+      case ST7735_RASET:
+        LOG(std::format("RASET: "));
+        this->set_state(new RasetState<width, height>());
+        break;
+      case ST7735_RAMWR:
+        LOG(std::format("RAMWR:\n"));
+        this->set_state(new RamWriteState<width, height>());
+        break;
+      case ST7735_NOP:
+      case ST7735_SWRESET:
+      case ST7735_RDDID:
+      case ST7735_RDDST:
+      case ST7735_SLPIN:
+      case ST7735_SLPOUT:
+      case ST7735_PTLON:
+      case ST7735_NORON:
+      case ST7735_INVOFF:
+      case ST7735_INVON:
+      case ST7735_DISPOFF:
+      case ST7735_DISPON:
+      case ST7735_PTLAR:
+      case ST7735_COLMOD:
+      case ST7735_MADCTL:
+      case ST7735_FRMCTR1:
+      case ST7735_FRMCTR2:
+      case ST7735_FRMCTR3:
+      case ST7735_INVCTR:
+      case ST7735_DISSET5:
+      case ST7735_PWCTR1:
+      case ST7735_PWCTR2:
+      case ST7735_PWCTR3:
+      case ST7735_PWCTR4:
+      case ST7735_PWCTR5:
+      case ST7735_VMCTR1:
+      case ST7735_RDID1:
+      case ST7735_RDID2:
+      case ST7735_RDID3:
+      case ST7735_RDID4:
+      case ST7735_PWCTR6:
+      case ST7735_GMCTRP1:
+      case ST7735_GMCTRN1:
+      case ST7735_RAMRD:
+      default:
+        LOG(std::format("cmd[{:#02x}] unimp: \n", cmd));
+        break;
+    }
+  }
+
+  void parse_caset(std::vector<uint8_t>& buffer) {
+    cursor.col = cursor.col_start = buffer[0] << 8 | buffer[1];
+    cursor.col_end                = buffer[2] << 8 | buffer[3];
+    LOG(std::format("x: {},y:{} \n", col_addr_s_, col_addr_e_));
+  }
+
+  void parse_raset(std::vector<uint8_t>& buffer) {
+    cursor.row = cursor.row_start = buffer[0] << 8 | buffer[1];
+    cursor.row_end                = buffer[2] << 8 | buffer[3];
+    LOG(std::format("x: {},y:{} \n", row_addr_s_, row_addr_e_));
+  }
+
+  void ram_write(std::vector<uint8_t>& buffer) {
+    for (size_t i = 0; i < buffer.size() - 1; i += 2) {
+      uint16_t bgr565 = buffer[i] << 8 | buffer[i + 1];
+      Pixel pixel    = {.rgb = {.r = static_cast<uint8_t>((((bgr565 >> 0) & 0x1f) << 3) | 0x7),
+                                .g = static_cast<uint8_t>((((bgr565 >> 5) & 0x3f) << 2) | 0x3),
+                                .b = static_cast<uint8_t>((((bgr565 >> 11) & 0x1f) << 3) | 0x7)}};
+
+      frame_buffer[cursor.row][cursor.col] = pixel;
+      cursor++;
+    }
+  }
+
+  void render() {
+    for (auto& row : frame_buffer) {
+      LOG(std::format("{{"));
+      for (auto& pixel : row) {
+        std::cout << std::format("{:02x}{:02x}{:02x},", pixel.rgb.r, pixel.rgb.g, pixel.rgb.b);
+      }
+      std::cout << std::format("}}\n");
+    }
+  }
+
+  void bmp(std::string filename) {
+    unsigned char bpm[width * height * 3];
+
+    size_t i = 0;
+    for (auto& row : frame_buffer) {
+      for (auto& pixel : row) {
+        bpm[i++] = pixel.rgb.r;
+        bpm[i++] = pixel.rgb.g;
+        bpm[i++] = pixel.rgb.b;
+      }
+    }
+    stbi_write_bmp(filename.c_str(), width, height, 3, bpm);
+  }
+
+  void png(std::string filename) {
+    unsigned char png[width * height * 3];
+
+    size_t i = 0;
+    for (auto& row : frame_buffer) {
+      for (auto& pixel : row) {
+        png[i++] = pixel.rgb.r;
+        png[i++] = pixel.rgb.g;
+        png[i++] = pixel.rgb.b;
+      }
+    }
+    stbi_write_png(filename.c_str(), width, height, 3, png, 3 * width);
+  }
+
+  void dc_pin(PinLevel level) {
+    if (level == PinLevel::Low) {
+      this->set_state(new CommandState<width, height>());
+    }
+    dc_pin_ = level;
+  }
+
+  void cs_pin(PinLevel level) { cs_pin_ = level; }
+};
+
+}  // namespakce Simulator
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..842eadb
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,8 @@
+add_library(${NAME} STATIC
+  "core/lucida_console_12pt.c"
+  "core/lcd_base.c"
+  "core/lucida_console_10pt.c"
+  "core/m3x6_16pt.c"
+  "st7735/lcd_st7735.c"
+)
+
diff --git a/core/font.h b/src/core/font.h
similarity index 90%
rename from core/font.h
rename to src/core/font.h
index 0cd07b0..6199269 100644
--- a/core/font.h
+++ b/src/core/font.h
@@ -7,6 +7,10 @@
 #ifndef COMMON_FONT_H
 #define COMMON_FONT_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdint.h>
 
 typedef struct FontCharInfo_st {
@@ -22,4 +26,8 @@ typedef struct Font_st {
   const unsigned char *bitmap_table;    /*< Character bitmap array. */
 } Font;
 
-#endif
\ No newline at end of file
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/core/lcd_base.c b/src/core/lcd_base.c
similarity index 100%
rename from core/lcd_base.c
rename to src/core/lcd_base.c
diff --git a/core/lcd_base.h b/src/core/lcd_base.h
similarity index 98%
rename from core/lcd_base.h
rename to src/core/lcd_base.h
index 04b4fd2..b16c5b2 100644
--- a/core/lcd_base.h
+++ b/src/core/lcd_base.h
@@ -6,6 +6,11 @@
 #ifndef DISPLAY_DRIVERS_COMMON_BASE_H_
 #define DISPLAY_DRIVERS_COMMON_BASE_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
@@ -192,4 +197,8 @@ static inline uint16_t LCD_rgb565_to_bgr565(const uint8_t rgb[2]) {
   return ENDIANESS_TO_HALF_WORD(color);
 }
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/core/lucida_console_10pt.c b/src/core/lucida_console_10pt.c
similarity index 100%
rename from core/lucida_console_10pt.c
rename to src/core/lucida_console_10pt.c
diff --git a/core/lucida_console_10pt.h b/src/core/lucida_console_10pt.h
similarity index 81%
rename from core/lucida_console_10pt.h
rename to src/core/lucida_console_10pt.h
index 35c08c4..9ed0648 100644
--- a/core/lucida_console_10pt.h
+++ b/src/core/lucida_console_10pt.h
@@ -6,6 +6,10 @@
 #ifndef LUCIDACONSOLE_10PT_H_
 #define LUCIDACONSOLE_10PT_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdint.h>
 
 #include "font.h"
@@ -15,4 +19,8 @@ extern const unsigned char lucidaConsole_10ptBitmaps[];
 extern const Font lucidaConsole_10ptFont;
 extern const FontCharInfo lucidaConsole_10ptDescriptors[];
 
-#endif /* LUCIDACONSOLE_10PT_H_ */
\ No newline at end of file
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* LUCIDACONSOLE_10PT_H_ */
diff --git a/core/lucida_console_12pt.c b/src/core/lucida_console_12pt.c
similarity index 100%
rename from core/lucida_console_12pt.c
rename to src/core/lucida_console_12pt.c
diff --git a/core/lucida_console_12pt.h b/src/core/lucida_console_12pt.h
similarity index 81%
rename from core/lucida_console_12pt.h
rename to src/core/lucida_console_12pt.h
index 5cd9bf9..b262463 100644
--- a/core/lucida_console_12pt.h
+++ b/src/core/lucida_console_12pt.h
@@ -6,6 +6,10 @@
 #ifndef LUCIDACONSOLE_12PT_H_
 #define LUCIDACONSOLE_12PT_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdint.h>
 
 #include "font.h"
@@ -15,4 +19,8 @@ extern const unsigned char lucidaConsole_12ptBitmaps[];
 extern const Font lucidaConsole_12ptFont;
 extern const FontCharInfo lucidaConsole_12ptDescriptors[];
 
-#endif /* LUCIDACONSOLE_12PT_H_ */
\ No newline at end of file
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* LUCIDACONSOLE_12PT_H_ */
diff --git a/core/m3x6_16pt.c b/src/core/m3x6_16pt.c
similarity index 100%
rename from core/m3x6_16pt.c
rename to src/core/m3x6_16pt.c
diff --git a/core/m3x6_16pt.h b/src/core/m3x6_16pt.h
similarity index 86%
rename from core/m3x6_16pt.h
rename to src/core/m3x6_16pt.h
index 29b9eef..9d34aaa 100644
--- a/core/m3x6_16pt.h
+++ b/src/core/m3x6_16pt.h
@@ -5,6 +5,10 @@
 #ifndef M3X6_16PT_H_
 #define M3X6_16PT_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdint.h>
 
 #include "font.h"
@@ -14,4 +18,8 @@ extern const unsigned char m3x6_16ptBitmaps[];
 extern const Font m3x6_16ptFont;
 extern const FontCharInfo m3x6_16ptDescriptors[];
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* M3X6_16PT_H_ */
diff --git a/st7735/lcd_st7735.c b/src/st7735/lcd_st7735.c
similarity index 100%
rename from st7735/lcd_st7735.c
rename to src/st7735/lcd_st7735.c
diff --git a/st7735/lcd_st7735.h b/src/st7735/lcd_st7735.h
similarity index 99%
rename from st7735/lcd_st7735.h
rename to src/st7735/lcd_st7735.h
index 301be0f..a76373e 100644
--- a/st7735/lcd_st7735.h
+++ b/src/st7735/lcd_st7735.h
@@ -6,6 +6,10 @@
 #ifndef DISPLAY_DRIVERS_ST7735_ST7735_H_
 #define DISPLAY_DRIVERS_ST7735_ST7735_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <stdint.h>
 
 #include "../core/font.h"
@@ -257,4 +261,9 @@ void lcd_st7735_set_frame_buffer_resolution(St7735Context *ctx, size_t width, si
  * @return Result of the operation.
  */
 Result lcd_st7735_check_frame_buffer_resolution(St7735Context *lcd, size_t *width, size_t *height);
+
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/st7735/lcd_st7735_cmds.h b/src/st7735/lcd_st7735_cmds.h
similarity index 97%
rename from st7735/lcd_st7735_cmds.h
rename to src/st7735/lcd_st7735_cmds.h
index 5762fa2..f5144a3 100644
--- a/st7735/lcd_st7735_cmds.h
+++ b/src/st7735/lcd_st7735_cmds.h
@@ -31,6 +31,10 @@
 #ifndef DISPLAY_DRIVERS_ST7735_ST7735_CMD_H_
 #define DISPLAY_DRIVERS_ST7735_ST7735_CMD_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 typedef enum {
   ST7735_NOP     = 0x00,
   ST7735_SWRESET = 0x01,
@@ -92,4 +96,8 @@ typedef enum {
   ST7735_ColorWhite   = 0xFFFF,
 } ST7735_Color;
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/st7735/lcd_st7735_init.h b/src/st7735/lcd_st7735_init.h
similarity index 99%
rename from st7735/lcd_st7735_init.h
rename to src/st7735/lcd_st7735_init.h
index 7664f7b..d500667 100644
--- a/st7735/lcd_st7735_init.h
+++ b/src/st7735/lcd_st7735_init.h
@@ -32,6 +32,10 @@
 #ifndef DISPLAY_DRIVERS_ST7735_ST7735_INIT_H_
 #define DISPLAY_DRIVERS_ST7735_ST7735_INIT_H_
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "lcd_st7735_cmds.h"
 #define NEXT_BYTE(addr) (*(const uint8_t *)(addr++))
 
@@ -160,5 +164,9 @@ static const uint8_t init_script_r3[] = {
       100 						//100 ms delay
 };
 
+#ifdef __cplusplus
+}
+#endif
+
 // clang-format on
 #endif
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
new file mode 100644
index 0000000..19b610c
--- /dev/null
+++ b/tests/CMakeLists.txt
@@ -0,0 +1,25 @@
+
+# Fetch GoogleTest library. 
+include(FetchContent)
+FetchContent_Declare(
+  googletest
+  URL https://github.com/google/googletest/archive/release-1.12.1.zip
+)
+FetchContent_MakeAvailable(googletest)
+
+FetchContent_Declare(FTB
+  GIT_REPOSITORY    https://github.com/nothings/stb
+  GIT_TAG           f0569113c93ad095470c54bf34a17b36646bbbb5
+)
+FetchContent_Populate(FTB)
+
+set(TEST_NAME ${NAME}_test)
+
+# add_compile_options(-O0 -g3)
+add_executable(${TEST_NAME} main.cc )
+
+target_include_directories(${TEST_NAME} PRIVATE "../" "${ftb_SOURCE_DIR}")
+target_link_libraries(${TEST_NAME} PRIVATE GTest::gtest_main ${NAME} )
+
+add_test(NAME Test_0 COMMAND ${TEST_NAME})
+
diff --git a/tests/golden_files/st7735_startup.txt b/tests/golden_files/st7735_startup.txt
new file mode 100644
index 0000000..a1a443b
--- /dev/null
+++ b/tests/golden_files/st7735_startup.txt
@@ -0,0 +1,189 @@
+gpio_write: cs=false, dc=false
+spi_write: 01 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 50
+gpio_write: cs=false, dc=false
+spi_write: 11 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 500
+gpio_write: cs=false, dc=false
+spi_write: 3a 
+gpio_write: cs=false, dc=true
+spi_write: 05 
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: b1 
+gpio_write: cs=false, dc=true
+spi_write: 00 06 03 
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: 36 
+gpio_write: cs=false, dc=true
+spi_write: 68 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: b6 
+gpio_write: cs=false, dc=true
+spi_write: 15 02 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: b4 
+gpio_write: cs=false, dc=true
+spi_write: 00 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c0 
+gpio_write: cs=false, dc=true
+spi_write: 02 70 
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: c1 
+gpio_write: cs=false, dc=true
+spi_write: 05 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c2 
+gpio_write: cs=false, dc=true
+spi_write: 01 02 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c5 
+gpio_write: cs=false, dc=true
+spi_write: 3c 38 
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: fc 
+gpio_write: cs=false, dc=true
+spi_write: 11 15 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: e0 
+gpio_write: cs=false, dc=true
+spi_write: 09 16 09 20 21 1b 13 19 17 15 1e 2b 04 05 02 0e 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: e1 
+gpio_write: cs=false, dc=true
+spi_write: 0b 14 08 1e 22 1d 18 1e 1b 1a 24 2b 06 06 02 0f 
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: 2a 
+gpio_write: cs=false, dc=true
+spi_write: 00 02 00 81 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 2b 
+gpio_write: cs=false, dc=true
+spi_write: 00 02 00 81 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 13 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: 29 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 500
+gpio_write: cs=false, dc=false
+spi_write: 01 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 150
+gpio_write: cs=false, dc=false
+spi_write: 11 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 500
+gpio_write: cs=false, dc=false
+spi_write: b1 
+gpio_write: cs=false, dc=true
+spi_write: 00 02 02 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: b2 
+gpio_write: cs=false, dc=true
+spi_write: 00 02 02 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: b3 
+gpio_write: cs=false, dc=true
+spi_write: 00 02 02 00 02 02 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: b4 
+gpio_write: cs=false, dc=true
+spi_write: 07 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c0 
+gpio_write: cs=false, dc=true
+spi_write: a2 02 84 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c1 
+gpio_write: cs=false, dc=true
+spi_write: c5 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c2 
+gpio_write: cs=false, dc=true
+spi_write: 0a 00 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c3 
+gpio_write: cs=false, dc=true
+spi_write: 8a 2a 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c4 
+gpio_write: cs=false, dc=true
+spi_write: 8a ee 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: c5 
+gpio_write: cs=false, dc=true
+spi_write: 0e 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 20 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 36 
+gpio_write: cs=false, dc=true
+spi_write: 68 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 3a 
+gpio_write: cs=false, dc=true
+spi_write: 05 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: e0 
+gpio_write: cs=false, dc=true
+spi_write: 02 1c 07 12 37 32 29 2d 29 25 2b 39 00 01 03 10 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: e1 
+gpio_write: cs=false, dc=true
+spi_write: 03 1d 07 06 2e 2c 29 2d 2e 2e 37 3f 00 00 02 10 
+gpio_write: cs=true, dc=true
+gpio_write: cs=false, dc=false
+spi_write: 13 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 10
+gpio_write: cs=false, dc=false
+spi_write: 29 
+gpio_write: cs=false, dc=true
+gpio_write: cs=true, dc=true
+sleep_ms: 100
diff --git a/tests/golden_files/test_draw_rectangles.png b/tests/golden_files/test_draw_rectangles.png
new file mode 100644
index 0000000..e995718
Binary files /dev/null and b/tests/golden_files/test_draw_rectangles.png differ
diff --git a/tests/golden_files/test_draw_text.png b/tests/golden_files/test_draw_text.png
new file mode 100644
index 0000000..95652f6
Binary files /dev/null and b/tests/golden_files/test_draw_text.png differ
diff --git a/tests/main.cc b/tests/main.cc
new file mode 100644
index 0000000..2d1c43e
--- /dev/null
+++ b/tests/main.cc
@@ -0,0 +1,288 @@
+#include <gtest/gtest.h>
+
+#include <cstdio>
+#include <cstring>
+#include <format>
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include "../src/st7735/lcd_st7735.h"
+#define STB_IMAGE_IMPLEMENTATION
+#include "stb_image.h"
+
+constexpr size_t DisplayWidth  = 160;
+constexpr size_t DisplayHeight = 128;
+
+void log_hex(std::ostream &stream, const uint8_t *data, size_t len) {
+  for (size_t i = 0; i < len; ++i) {
+    stream << std::format("{:02x} ", static_cast<int>(data[i]));
+  }
+  stream << std::endl;
+}
+
+struct MockInterfaceFile {
+  std::string filename_;
+
+  MockInterfaceFile() {
+    char buffer[1024];
+    filename_ = std::string(std::tmpnam(buffer));
+  };
+
+  static uint32_t spi_write(void *handle, uint8_t *data, size_t len) {
+    MockInterfaceFile *self = (MockInterfaceFile *)handle;
+    std::ofstream file(self->filename_, std::ios::app);
+    file << std::format("{}: ", __func__);
+    log_hex(file, data, len);
+    return len;
+  }
+
+  static uint32_t gpio_write(void *handle, bool cs, bool dc) {
+    MockInterfaceFile *self = (MockInterfaceFile *)handle;
+    std::ofstream file(self->filename_, std::ios::app);
+    file << std::format("{}: cs={}, dc={}", __func__, cs, dc) << std::endl;
+    return 0;
+  }
+
+  static uint32_t reset(void *handle) {
+    MockInterfaceFile *self = (MockInterfaceFile *)handle;
+    std::ofstream file(self->filename_, std::ios::app);
+    file << std::format("{}", __func__) << std::endl;
+    return 0;
+  }
+
+  static void set_pwm(void *handle, uint8_t pwm) {
+    MockInterfaceFile *self = (MockInterfaceFile *)handle;
+    std::ofstream file(self->filename_, std::ios::app);
+    file << std::format("{}: {}", __func__, pwm) << std::endl;
+  }
+
+  static void sleep_ms(void *handle, uint32_t ms) {
+    MockInterfaceFile *self = (MockInterfaceFile *)handle;
+    std::ofstream file(self->filename_, std::ios::app);
+    file << std::format("{}: {}", __func__, ms) << std::endl;
+  }
+};
+
+class DisplayTest : public testing::Test {
+ public:
+#define GET_GOLDEN_FILE()                                                                                              \
+  std::format("./tests/golden_files/test_{}.png", testing::UnitTest::GetInstance()->current_test_info()->name())
+
+  template<typename T>
+  T rotate_left(T& v) {
+    v = v << 8 | v >> (32 - 8); 
+    return v;
+  }
+  std::string make_temp_filename() {
+    char buffer[1024];
+    return std::string(std::tmpnam(buffer));
+  }
+
+  void compare_img(const std::string &result_img, const std::string &expected_img) {
+    int w1, h1, c1, w2, h2, c2;
+
+    unsigned char *result   = stbi_load(result_img.c_str(), &w1, &h1, &c1, 0);
+    unsigned char *expected = stbi_load(expected_img.c_str(), &w2, &h2, &c2, 0);
+
+    ASSERT_NE(expected, nullptr) << std::format("File {} not found, to compare with {}", expected_img, result_img);
+    ASSERT_NE(result, nullptr) << std::format("File {} not found, to compare with {}", result_img, expected_img);
+    ASSERT_TRUE(w1 == w2 && h1 == h2 && c1 == c2);
+
+    size_t size = w1 * h1 * c1;
+    ASSERT_TRUE(std::memcmp(result, expected, size) == 0)
+        << std::format("Mismatch {} != {}\n", result_img, expected_img)
+        << std::format("Run: compare {} {} /tmp/diff.png", result_img, expected_img);
+
+    stbi_image_free(result);
+    stbi_image_free(expected);
+  }
+
+  void compare_files(const std::string &result_file, const std::string &expected_file) {
+    std::ifstream f1(result_file), f2(expected_file);
+    std::string result, expected;
+    int lineNum = 1;
+
+    while (std::getline(f1, result) && std::getline(f2, expected)) {
+      EXPECT_EQ(result, expected) << std::format("File mismatch {} != {} at line{}", result_file, expected_file,
+                                                 lineNum);
+      ++lineNum;
+    }
+  }
+};
+
+class st7735Test : public DisplayTest {
+ public:
+  MockInterfaceFile mock_ = MockInterfaceFile();
+  St7735Context ctx_;
+  LCD_Interface interface_;
+
+  st7735Test() {
+    interface_ = {
+        .handle            = &mock_,
+        .spi_write         = MockInterfaceFile::spi_write,
+        .spi_read          = NULL,
+        .gpio_write        = MockInterfaceFile::gpio_write,
+        .reset             = MockInterfaceFile::reset,
+        .set_backlight_pwm = MockInterfaceFile::set_pwm,
+        .timer_delay       = MockInterfaceFile::sleep_ms,
+    };
+    lcd_st7735_init(&ctx_, &interface_);
+  }
+};
+
+TEST_F(st7735Test, startup) {
+  Result res = lcd_st7735_startup(&ctx_);
+  EXPECT_EQ(res.code, 0);
+  compare_files(mock_.filename_, "./golden_files/st7735_startup.txt");
+}
+
+#include <simulator/st7735/controller.hh>
+struct MockInterfaceSimulator {
+  Simulator::St7735<DisplayWidth, DisplayHeight> simulator;
+
+  MockInterfaceSimulator() {};
+
+  static uint32_t spi_write(void *handle, uint8_t *data, size_t len) {
+    MockInterfaceSimulator *self = (MockInterfaceSimulator *)handle;
+    self->simulator.spi_write(data, len);
+    return len;
+  }
+
+  static uint32_t gpio_write(void *handle, bool cs, bool dc) {
+    MockInterfaceSimulator *self = (MockInterfaceSimulator *)handle;
+    self->simulator.dc_pin(dc ? Simulator::PinLevel::High : Simulator::PinLevel::Low);
+    self->simulator.cs_pin(cs ? Simulator::PinLevel::High : Simulator::PinLevel::Low);
+    return 0;
+  }
+
+  static uint32_t reset(void *handle) {
+    MockInterfaceSimulator *self = (MockInterfaceSimulator *)handle;
+    return 0;
+  }
+
+  static void set_pwm(void *handle, uint8_t pwm) { MockInterfaceSimulator *self = (MockInterfaceSimulator *)handle; }
+
+  static void sleep_ms(void *handle, uint32_t ms) { MockInterfaceSimulator *self = (MockInterfaceSimulator *)handle; }
+};
+
+class st7735SimTest : public DisplayTest {
+ public:
+  MockInterfaceSimulator mock_ = MockInterfaceSimulator();
+  St7735Context ctx_;
+  LCD_Interface interface_;
+
+  st7735SimTest() {
+    interface_ = {
+        .handle            = &mock_,
+        .spi_write         = MockInterfaceSimulator::spi_write,
+        .spi_read          = NULL,
+        .gpio_write        = MockInterfaceSimulator::gpio_write,
+        .reset             = MockInterfaceSimulator::reset,
+        .set_backlight_pwm = MockInterfaceSimulator::set_pwm,
+        .timer_delay       = MockInterfaceSimulator::sleep_ms,
+    };
+    lcd_st7735_init(&ctx_, &interface_);
+    lcd_st7735_startup(&ctx_);
+  }
+};
+
+TEST_F(st7735SimTest, draw_rectangles) {
+  Result res = lcd_st7735_clean(&ctx_);
+  EXPECT_EQ(res.code, 0);
+  size_t increment = 20;
+  uint32_t rgb = 0x0000FF;
+
+  {
+    LCD_rectangle rec{.origin = {.x = 0, .y = 0}, .width = DisplayWidth, .height = 10};
+    auto draw_horizontal = [&](uint32_t color) {
+      rec.origin.y += increment;
+      res = lcd_st7735_fill_rectangle(&ctx_, rec, color);
+      EXPECT_EQ(res.code, 0);
+    };
+
+    draw_horizontal(rotate_left(rgb));
+    draw_horizontal(rotate_left(rgb));
+    draw_horizontal(rotate_left(rgb));
+    draw_horizontal(rotate_left(rgb));
+    draw_horizontal(rotate_left(rgb));
+  }
+  {
+    LCD_rectangle rec{.origin = {.x = 10, .y = 0}, .width = 10, .height = DisplayHeight};
+    auto draw_vertical = [&](uint32_t color) {
+      rec.origin.x += increment;
+      res = lcd_st7735_fill_rectangle(&ctx_, rec, color);
+      EXPECT_EQ(res.code, 0);
+    };
+
+    draw_vertical(rotate_left(rgb));
+    draw_vertical(rotate_left(rgb));
+    draw_vertical(rotate_left(rgb));
+    draw_vertical(rotate_left(rgb));
+    draw_vertical(rotate_left(rgb));
+    draw_vertical(rotate_left(rgb));
+
+    EXPECT_EQ(res.code, 0);
+  }
+
+  std::string filename = make_temp_filename();
+  mock_.simulator.png(filename);
+  compare_img(filename, GET_GOLDEN_FILE());
+}
+
+#include <src/core/lucida_console_10pt.h>
+#include <src/core/lucida_console_12pt.h>
+TEST_F(st7735SimTest, draw_text) {
+  Result res = lcd_st7735_clean(&ctx_);
+  EXPECT_EQ(res.code, 0);
+
+  std::string ascii = "";
+  for (int i = 32; i <= 126; ++i) {
+    if (std::isprint(static_cast<unsigned char>(i))) {
+      ascii += static_cast<char>(i);
+    }
+  }
+  size_t font_h(0), rows(0), columns(0), ascii_index(0);
+  uint32_t bg(0xff), fg(0x00);
+  LCD_Point pos{.x = 0, .y = 0};
+
+  auto set_font = [&](const Font *font) {
+    res = lcd_st7735_set_font(&ctx_, font);
+    EXPECT_EQ(res.code, 0);
+    font_h      = ctx_.parent.font->height;
+    rows        = DisplayHeight / ctx_.parent.font->height;
+    columns     = DisplayWidth / ctx_.parent.font->descriptor_table->width;
+    ascii_index = 0;
+  };
+
+  auto draw_text = [&]() {
+    res = lcd_st7735_set_font_colors(&ctx_, bg, fg);
+    EXPECT_EQ(res.code, 0);
+    std::string print = ascii.substr(ascii_index, columns);
+    res               = lcd_st7735_puts(&ctx_, pos, print.c_str());
+    EXPECT_EQ(res.code, print.size());
+    ascii_index += columns;
+    pos.y += font_h;
+    bg = bg << 8 | bg >> (32 - 8);  // Rotate left
+    fg = bg ^ 0xffffff;
+  };
+
+  set_font(&lucidaConsole_12ptFont);
+  do {
+    draw_text();
+  } while (ascii_index < ascii.size());
+
+  set_font(&lucidaConsole_10ptFont);
+  do {
+    draw_text();
+  } while (pos.y < DisplayHeight - font_h);
+
+  std::string filename = make_temp_filename();
+  mock_.simulator.png(filename);
+  compare_img(filename, GET_GOLDEN_FILE());
+}
+
+int main(int argc, char **argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}