diff --git a/platformio.ini b/platformio.ini
index d0defb1..f287453 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -6,7 +6,7 @@ upload_speed = 2000000
 monitor_speed = 115200
 board_build.partitions = default_16MB.csv
 build_flags = 
-  -std=gnu++20
+  -std=gnu++2a
   -Ofast
   -DBOARD_HAS_PSRAM
   -mfix-esp32-psram-cache-issue
diff --git a/src/keypad.cpp b/src/keypad.cpp
new file mode 100644
index 0000000..84de111
--- /dev/null
+++ b/src/keypad.cpp
@@ -0,0 +1,203 @@
+#define LGFX_M5PAPER
+#define LGFX_USE_V1
+#define LGFX_AUTODETECT
+
+#include "keypad.hpp"
+#include <M5EPD.h>
+#include <LovyanGFX.hpp>
+
+static LGFX gfx;
+
+constexpr float FONT_SIZE_NORMAL = 4.0;
+constexpr float FONT_SIZE_LARGE = 8.0;
+
+static const float_t BUTTON_WIDTH = 115;
+static const float_t BUTTON_HEIGHT = 115;
+static int last_index = -1;
+
+static String oldinput = "";
+static String input = "";
+
+int index(int x, int y)
+{
+    for (int index = 0; index <= 11; index++)
+    {
+        float_t cx = (125 * (index % 3)) + 15;
+        float_t cy = (125 * (index / 3)) + 20;
+
+        if (x > cx && x < cx + BUTTON_WIDTH && y > cy && y < cy + BUTTON_HEIGHT)
+        {
+            return index;
+        }
+    }
+
+    return -1;
+}
+
+void drawButton(int index, String label)
+{
+    float_t x = (125 * (index % 3)) + 15;
+    float_t y = (125 * (index / 3)) + 20;
+
+    gfx.setCursor(x + (115 / 2) - (gfx.fontWidth() / 2), y + (115 / 2) - (gfx.fontHeight() / 2));
+    gfx.print(label);
+
+    gfx.setColor(TFT_BLACK);
+    gfx.drawFastHLine(x, y, BUTTON_WIDTH);
+    gfx.drawFastHLine(x, y + BUTTON_HEIGHT, BUTTON_WIDTH);
+    gfx.drawFastVLine(x, y, BUTTON_HEIGHT);
+    gfx.drawFastVLine(x + BUTTON_WIDTH, y, BUTTON_HEIGHT);
+}
+
+void drawButtons()
+{
+    gfx.setTextSize(FONT_SIZE_NORMAL);
+    for (int i = 0; i <= 11; i++)
+    {
+        String label = String(i + 1);
+        if (i == 9)
+        {
+            label = String("<");
+        }
+        if (i == 10)
+        {
+            label = String("0");
+        }
+        if (i == 11)
+        {
+            label = String(">");
+        }
+
+        drawButton(i, label);
+    }
+}
+
+void drawInputField()
+{
+    int x = 405;
+    int y = 20;
+
+    gfx.setColor(TFT_BLACK);
+    gfx.drawFastHLine(x, y, 520);
+    gfx.drawFastHLine(x, y + 115, 520);
+    gfx.drawFastVLine(x, y, 115);
+    gfx.drawFastVLine(x + 520, y, 115);
+}
+
+void drawInput()
+{
+    int x = 405;
+    int y = 20;
+
+    gfx.setEpdMode(epd_mode_t::epd_text);
+    gfx.setColor(TFT_BLACK);
+    gfx.setTextSize(FONT_SIZE_LARGE);
+
+    if (input.length() < oldinput.length())
+    {
+        gfx.setColor(TFT_WHITE);
+        gfx.fillRect(x + 1, y + 1, 519, 114);
+        gfx.setColor(TFT_BLACK);
+    }
+
+    gfx.setCursor(x + (520 / 2) - ((gfx.fontWidth() * input.length()) / 2), y + (115 / 2) - (gfx.fontHeight() / 2));
+    String keypad = "";
+    for (int i = 0; i < input.length(); i++)
+    {
+        keypad += "*";
+    }
+    gfx.print(keypad);
+    gfx.setEpdMode(epd_mode_t::epd_fast);
+}
+
+void keypad_init()
+{
+    gfx.init();
+    gfx.setRotation(1);
+
+    gfx.setEpdMode(epd_mode_t::epd_quality);
+    gfx.fillScreen(TFT_WHITE);
+    gfx.waitDisplay();
+    gfx.fillScreen(TFT_WHITE);
+    gfx.setEpdMode(epd_mode_t::epd_fast);
+
+    gfx.setColor(TFT_BLACK);
+    gfx.waitDisplay();
+
+    drawButtons();
+    drawInputField();
+    drawInput();
+}
+
+void keypad_write(String payload)
+{
+    payload.replace("_", " ");
+
+    gfx.setTextSize(FONT_SIZE_NORMAL);
+
+    gfx.setColor(TFT_WHITE);
+    gfx.fillRect(405, 155, gfx.fontWidth() * 10, gfx.fontHeight());
+    gfx.setColor(TFT_BLACK);
+
+    gfx.setCursor(405, 155);
+    payload[0] = toupper(payload[0]);
+    gfx.printf("%s", payload);
+}
+
+std::optional<String> checkForInput()
+{
+    if (M5.TP.available())
+    {
+        M5.TP.update();
+        if (M5.TP.isFingerUp())
+        {
+            last_index = -1;
+            return std::nullopt;
+        }
+
+        tp_finger_t fingerItem = M5.TP.readFinger(0);
+
+        int i = index(fingerItem.x, fingerItem.y);
+        if (last_index != i)
+        {
+            last_index = i;
+            if (i == 9)
+            {
+                // revert
+                input = "";
+            }
+            else if (i == 10)
+            {
+                if (input.length() >= 6)
+                    return std::nullopt;
+                input += String(0);
+            }
+            else if (i == 11)
+            {
+                // submit
+                String retval = input;
+                input = "";
+                return retval;
+            }
+            else
+            {
+                if (input.length() >= 6)
+                    return std::nullopt;
+                input += String(i + 1);
+            }
+        }
+    }
+    return std::nullopt;
+}
+
+std::optional<String> keypad_loop()
+{
+    auto value = checkForInput();
+    if (oldinput != input)
+    {
+        drawInput();
+        oldinput = input;
+    }
+
+    return value;
+}
\ No newline at end of file
diff --git a/src/keypad.hpp b/src/keypad.hpp
new file mode 100644
index 0000000..6b509bf
--- /dev/null
+++ b/src/keypad.hpp
@@ -0,0 +1,11 @@
+#ifndef _KEYPAD_H_
+#define _KEYPAD_H_
+
+#include <WString.h>
+#include <optional>
+
+void keypad_init();
+void keypad_write(String);
+std::optional<String> keypad_loop();
+
+#endif
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index f2017d4..3a733cc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,155 +3,33 @@
 
 #define YAML_DISABLE_CJSON
 
-#include <ArduinoJson.h>
-#include <ArduinoYaml.h>
 #include <WiFi.h>
 #include <M5EPD.h>
 #include <NTPClient.h>
 #include <HTTPClient.h>
-#include <PubSubClient.h>
 
 #include <totp.hpp>
 
-#define LGFX_M5PAPER
-#define LGFX_USE_V1
-#define LGFX_AUTODETECT
+#include "settings.hpp"
+#include "mqtt.hpp"
+#include "keypad.hpp"
 
-#include <LovyanGFX.hpp>
-
-static LGFX gfx;
 static WiFiUDP ntpUDP;
 static NTPClient timeClient(ntpUDP);
-
-constexpr float FONT_SIZE_NORMAL = 4.0;
-constexpr float FONT_SIZE_LARGE = 8.0;
-
-static YAMLNode *SETTINGS = nullptr;
-
-static String oldinput = "";
-static String input = "";
-
-static int last_index = -1;
+static MQTT *mqtt;
 
 static bool is_disarmed = false;
 
 static const int WIFI_CONNECT_RETRY_MAX = 30;
-static const float_t BUTTON_WIDTH = 115;
-static const float_t BUTTON_HEIGHT = 115;
-
-void drawLockState(String state)
-{
-  state.replace("_", " ");
-
-  gfx.setTextSize(FONT_SIZE_NORMAL);
-
-  gfx.setColor(TFT_WHITE);
-  gfx.fillRect(405, 155, gfx.fontWidth() * 10, gfx.fontHeight());
-  gfx.setColor(TFT_BLACK);
-
-  gfx.setCursor(405, 155);
-  state[0] = toupper(state[0]);
-  gfx.printf("%s", state);
-}
 
 void mqtt_callback(char *topic, byte *payload, unsigned int length)
 {
-  String state;
-  for (int i = 0; i < length; i++)
-  {
-    state += (const char)payload[i];
-  }
-
+  payload[length] = '\0';
+  String state((const char *)payload);
   is_disarmed = state == "disarmed";
 
-  Serial.printf("%d: %s - %s\n", length, topic, state);
-  drawLockState(state);
-}
-
-WiFiClient wifiClient;
-PubSubClient *client = nullptr;
-
-void cls()
-{
-  gfx.setEpdMode(epd_mode_t::epd_quality);
-  gfx.fillScreen(TFT_WHITE);
-  gfx.waitDisplay();
-  gfx.fillScreen(TFT_WHITE);
-  gfx.setEpdMode(epd_mode_t::epd_fast);
-  gfx.setColor(TFT_BLACK);
-  gfx.waitDisplay();
-}
-
-void drawButton(int index, String label)
-{
-  float_t x = (125 * (index % 3)) + 15;
-  float_t y = (125 * (index / 3)) + 20;
-
-  gfx.setCursor(x + (115 / 2) - (gfx.fontWidth() / 2), y + (115 / 2) - (gfx.fontHeight() / 2));
-  gfx.print(label);
-
-  gfx.setColor(TFT_BLACK);
-  gfx.drawFastHLine(x, y, BUTTON_WIDTH);
-  gfx.drawFastHLine(x, y + BUTTON_HEIGHT, BUTTON_WIDTH);
-  gfx.drawFastVLine(x, y, BUTTON_HEIGHT);
-  gfx.drawFastVLine(x + BUTTON_WIDTH, y, BUTTON_HEIGHT);
-}
-
-void drawButtons()
-{
-  gfx.setTextSize(FONT_SIZE_NORMAL);
-  for (int i = 0; i <= 11; i++)
-  {
-    String label = String(i + 1);
-    if (i == 9)
-    {
-      label = String("<");
-    }
-    if (i == 10)
-    {
-      label = String("0");
-    }
-    if (i == 11)
-    {
-      label = String(">");
-    }
-
-    drawButton(i, label);
-  }
-}
-
-void drawInputField()
-{
-  int x = 405;
-  int y = 20;
-
-  gfx.setColor(TFT_BLACK);
-  gfx.drawFastHLine(x, y, 520);
-  gfx.drawFastHLine(x, y + 115, 520);
-  gfx.drawFastVLine(x, y, 115);
-  gfx.drawFastVLine(x + 520, y, 115);
-}
-
-void drawInput()
-{
-  int x = 405;
-  int y = 20;
-
-  gfx.setColor(TFT_WHITE);
-  gfx.fillRect(x + 1, y + 1, 519, 114);
-  gfx.setColor(TFT_BLACK);
-
-  gfx.setEpdMode(epd_mode_t::epd_text);
-  gfx.setColor(TFT_BLACK);
-  gfx.setTextSize(FONT_SIZE_LARGE);
-  gfx.setCursor(x + (520 / 2) - ((gfx.fontWidth() * input.length()) / 2), y + (115 / 2) - (gfx.fontHeight() / 2));
-  String display = "";
-  for (int i = 0; i < input.length(); i++)
-  {
-    display += "*";
-  }
-  gfx.print(display);
-  gfx.setEpdMode(epd_mode_t::epd_fast);
+  Serial.println(state);
+  keypad_write(state);
 }
 
 void setupTime()
@@ -159,55 +37,38 @@ void setupTime()
   timeClient.begin();
 }
 
-void send(String state)
-{
-  if (SETTINGS == nullptr)
-  {
-    return;
-  }
-
-  if (!WiFi.isConnected())
-  {
-    resetWifi();
-  }
-
-  if (client->connect(SETTINGS->gettext("mqtt:ident"), SETTINGS->gettext("mqtt:username"), SETTINGS->gettext("mqtt:password")))
-  {
-    client->unsubscribe(SETTINGS->gettext("mqtt:state_topic"));
-    client->subscribe(SETTINGS->gettext("mqtt:state_topic"));
-    client->publish(SETTINGS->gettext("mqtt:command_topic"), state.c_str());
-  }
-}
-
 void lock()
 {
   if (is_disarmed)
   {
-    send("ARM_AWAY");
+    mqtt->send("ARM_AWAY");
   }
 }
 
 void unlock()
 {
-  send("DISARM");
+  mqtt->send("DISARM");
 }
 
 bool isConnecting = false;
-void resetWifi()
+void initWifi(YAMLNode *settings)
 {
-  if (!SETTINGS)
+  if (!settings)
   {
+    keypad_write("no settings");
     return;
   }
 
   if (isConnecting)
   {
+    keypad_write("already connecting");
     return;
   }
-  isConnecting = true;
-  drawLockState("connecting");
 
-  WiFi.begin(SETTINGS->gettext("wifi:ssid"), SETTINGS->gettext("wifi:password"));
+  isConnecting = true;
+  keypad_write("connecting");
+
+  WiFi.begin(settings->gettext("wifi:ssid"), settings->gettext("wifi:password"));
 
   if (WiFi.isConnected())
   {
@@ -227,52 +88,20 @@ void resetWifi()
     Serial.print("Local IP: ");
     Serial.println(WiFi.localIP());
     isConnecting = false;
+    keypad_write("connected");
   }
   else
   {
-    drawLockState("failed to connect");
+    keypad_write("failed to connect");
     isConnecting = false;
     return;
   }
-
-  IPAddress ip;
-  if (!ip.fromString(SETTINGS->gettext("mqtt:hostname")))
-  {
-    drawLockState("failed to parse mqtt ip");
-    isConnecting = false;
-    return;
-  }
-
-  client = new PubSubClient(ip, 1883, mqtt_callback, wifiClient);
-
-  if (client->connect(SETTINGS->gettext("mqtt:ident"), SETTINGS->gettext("mqtt:username"), SETTINGS->gettext("mqtt:password")))
-  {
-    client->unsubscribe(SETTINGS->gettext("mqtt:state_topic"));
-    client->subscribe(SETTINGS->gettext("mqtt:state_topic"));
-  }
 }
 
-bool initSD()
-{
-  if (!SD.exists("/settings.yml"))
-  {
-    Serial.println("settings.yml not found");
-    drawLockState("settings.yml not found");
-    return false;
-  }
-
-  File settingsFile = SD.open("/settings.yml");
-  YAMLNode node = YAMLNode::loadStream(settingsFile);
-  settingsFile.close();
-  SETTINGS = new YAMLNode(node);
-
-  return true;
-}
-
-void initTOTP()
+void initTOTP(YAMLNode *settings)
 {
   uint8_t *base32_key = new uint8_t[20];
-  const char *hmac = ((String)SETTINGS->gettext("totp:hmac")).c_str();
+  const char *hmac = ((String)settings->gettext("totp:hmac")).c_str();
 
   for (int i = 0; i < 20; i++)
   {
@@ -286,40 +115,25 @@ void setup()
 {
   M5.begin();
 
-  gfx.init();
-  gfx.setEpdMode(epd_mode_t::epd_fast);
-  gfx.setRotation(1);
-  cls();
+  keypad_init();
+  keypad_write("loading");
 
-  drawButtons();
-  drawInputField();
-  drawInput();
+  auto settings = settings_load();
 
-  if (!initSD())
-    return;
-
-  resetWifi();
-  sleep(1);
-  setupTime();
-  initTOTP();
-
-  lock();
-}
-
-int index(int x, int y)
-{
-  for (int index = 0; index <= 11; index++)
+  if (!settings)
   {
-    float_t cx = (125 * (index % 3)) + 15;
-    float_t cy = (125 * (index / 3)) + 20;
-
-    if (x > cx && x < cx + BUTTON_WIDTH && y > cy && y < cy + BUTTON_HEIGHT)
-    {
-      return index;
-    }
+    keypad_write("unable to load settings");
+    return;
   }
 
-  return -1;
+  mqtt = new MQTT(settings, mqtt_callback);
+
+  initWifi(settings);
+  mqtt->connect();
+  setupTime();
+  initTOTP(settings);
+
+  // lock();
 }
 
 void submit(String code)
@@ -336,69 +150,24 @@ void submit(String code)
   lock();
 }
 
-void checkForInput()
-{
-  if (M5.TP.available())
-  {
-    M5.TP.update();
-    if (M5.TP.isFingerUp())
-    {
-      last_index = -1;
-      return;
-    }
-
-    tp_finger_t fingerItem = M5.TP.readFinger(0);
-
-    int i = index(fingerItem.x, fingerItem.y);
-    if (last_index != i)
-    {
-      last_index = i;
-      if (i == 9)
-      {
-        input = input.substring(0, input.length() - 1);
-      }
-      else if (i == 10)
-      {
-        input += String(0);
-      }
-      else if (i == 11)
-      {
-        // submit
-        submit(input);
-        input = "";
-      }
-      else
-      {
-        input += String(i + 1);
-      }
-    }
-  }
-}
-
-unsigned long lastBtnPressed = millis();
-
-void checkForButton()
-{
-  M5.BtnP.read();
-  if (M5.BtnP.isPressed() && millis() - lastBtnPressed > 1000)
-  {
-    lastBtnPressed = millis(); // try and debouce, not really working i guess
-    resetWifi();
-  }
-}
+// unsigned long lastBtnPressed = millis();
+// void checkForButton()
+// {
+//   M5.BtnP.read();
+//   if (M5.BtnP.isPressed() && millis() - lastBtnPressed > 1000)
+//   {
+//     lastBtnPressed = millis(); // try and debouce, not really working i guess
+//     initWifi();
+//   }
+// }
 
 void loop()
 {
-  checkForButton();
   timeClient.update();
-  if (client != nullptr)
-  {
-    client->loop();
-  }
-  checkForInput();
-  if (oldinput != input)
-  {
-    oldinput = input;
-    drawInput();
-  }
+  mqtt->loop();
+
+  // checkForButton();
+  auto value = keypad_loop();
+  if (value.has_value())
+    submit(value.value());
 }
diff --git a/src/mqtt.cpp b/src/mqtt.cpp
new file mode 100644
index 0000000..50b4782
--- /dev/null
+++ b/src/mqtt.cpp
@@ -0,0 +1,40 @@
+#include "mqtt.hpp"
+#include "settings.hpp"
+
+#include <WiFiClient.h>
+
+WiFiClient wifiClient;
+
+MQTT::MQTT(YAMLNode *settings, MQTT_CALLBACK_SIGNATURE)
+{
+    IPAddress ip;
+    if (!ip.fromString(settings->gettext("mqtt:hostname")))
+        return;
+
+    this->settings = settings;
+    this->client = new PubSubClient(ip, 1883, callback, wifiClient);
+}
+
+void MQTT::connect()
+{
+    this->client->connect(this->settings->gettext("mqtt:ident"), this->settings->gettext("mqtt:username"), this->settings->gettext("mqtt:password"));
+    this->client->unsubscribe(this->settings->gettext("mqtt:state_topic"));
+    this->client->subscribe(this->settings->gettext("mqtt:state_topic"));
+}
+
+void MQTT::send(String payload)
+{
+    client->publish(this->settings->gettext("mqtt:command_topic"), payload.c_str());
+}
+
+void MQTT::loop()
+{
+    if (client != nullptr)
+    {
+        if (!client->connected())
+        {
+            client->connect(this->settings->gettext("mqtt:ident"), this->settings->gettext("mqtt:username"), this->settings->gettext("mqtt:password"));
+        }
+        client->loop();
+    }
+}
diff --git a/src/mqtt.hpp b/src/mqtt.hpp
new file mode 100644
index 0000000..02329d2
--- /dev/null
+++ b/src/mqtt.hpp
@@ -0,0 +1,24 @@
+#ifndef _MQTT_H_
+#define _MQTT_H_
+
+#include <WString.h>
+#include <PubSubClient.h>
+
+#include <ArduinoJson.h>
+#include <ArduinoYaml.h>
+
+class MQTT
+{
+
+public:
+    MQTT(YAMLNode *settings, MQTT_CALLBACK_SIGNATURE);
+    void connect();
+    void send(String);
+    void loop();
+
+private:
+    YAMLNode *settings = nullptr;
+    PubSubClient *client = nullptr;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/settings.cpp b/src/settings.cpp
new file mode 100644
index 0000000..8309194
--- /dev/null
+++ b/src/settings.cpp
@@ -0,0 +1,18 @@
+#include "settings.hpp"
+#include <SD.h>
+
+YAMLNode *settings_load()
+{
+    Serial.println("Loading settings");
+    if (!SD.exists("/settings.yml"))
+    {
+        Serial.println("settings.yml not found");
+        return nullptr;
+    }
+
+    File settingsFile = SD.open("/settings.yml");
+    YAMLNode node = YAMLNode::loadStream(settingsFile);
+    settingsFile.close();
+
+    return new YAMLNode(node);
+}
\ No newline at end of file
diff --git a/src/settings.hpp b/src/settings.hpp
new file mode 100644
index 0000000..bea5aa4
--- /dev/null
+++ b/src/settings.hpp
@@ -0,0 +1,9 @@
+#ifndef _SETTINGS_HPP_
+#define _SETTINGS_HPP_
+
+#include <ArduinoJson.h>
+#include <ArduinoYaml.h>
+
+YAMLNode *settings_load();
+
+#endif
\ No newline at end of file