diff --git a/LibreMetric.ino b/LibreMetric.ino
index 4dea677..0fefa4e 100644
--- a/LibreMetric.ino
+++ b/LibreMetric.ino
@@ -48,6 +48,25 @@ String message, webpage;
 ESP8266WebServer server(PORT);
 Max72xxPanel matrix = Max72xxPanel(pinCS, numberOfHorizontalDisplays, numberOfVerticalDisplays);
 
+// in a terminal: telnet esp IP
+#ifdef DEBUG_TELNET
+WiFiServer  telnetServer(23);
+WiFiClient  telnetClient;
+
+void handleTelnet(void) {
+  if (telnetServer.hasClient()) {
+    if (!telnetClient || !telnetClient.connected()) {
+      if (telnetClient) {
+        telnetClient.stop();
+      }
+      telnetClient = telnetServer.available();
+    } else {
+      telnetServer.available().stop();
+    }
+  }
+}
+#endif
+
 void configModeCallback (WiFiManager *myWiFiManager) { // AP config mode
   display_message("Setup");
 }
@@ -152,6 +171,11 @@ void loop() {
   yield();
   ArduinoOTA.handle();
   yield();
+
+  #ifdef DEBUG_TELNET
+    // Handle Telnet connection for debugging
+    handleTelnet();
+  #endif
 }
 void display_message(String message){
   if (width * message.length() - spacer > matrix.width()) {
diff --git a/d_helper.cpp b/d_helper.cpp
index 1d14ca3..a1fb709 100644
--- a/d_helper.cpp
+++ b/d_helper.cpp
@@ -2,30 +2,3 @@
 
 #include "d_helper.h"
 
-// in a terminal: telnet esp IP
-#ifdef DEBUG_TELNET
-WiFiServer  telnetServer(23);
-WiFiClient  telnetClient;
-#endif
-
-///////////////////////////////////////////////////////////////////////////
-//   TELNET
-///////////////////////////////////////////////////////////////////////////
-/*
-   Function called to handle Telnet clients
-   https://www.youtube.com/watch?v=j9yW10OcahI
-*/
-#ifdef DEBUG_TELNET
-void handleTelnet(void) {
-  if (telnetServer.hasClient()) {
-    if (!telnetClient || !telnetClient.connected()) {
-      if (telnetClient) {
-        telnetClient.stop();
-      }
-      telnetClient = telnetServer.available();
-    } else {
-      telnetServer.available().stop();
-    }
-  }
-}
-#endif
diff --git a/d_helper.h b/d_helper.h
index f619153..0da41ad 100644
--- a/d_helper.h
+++ b/d_helper.h
@@ -1,22 +1,22 @@
 #ifndef D_helper_h
-#define D_helper_h
+  #define D_helper_h
 
-void handleTelnet(void);
-
-// ################# Macros for debugging ################
-
-#ifdef DEBUG_TELNET
-#define     DEBUG_PRINT(x)                  telnetClient.print(x)
-#define     DEBUG_PRINTF(x,y)               telnetClient.printf(x,y)
-#define     DEBUG_PRINT_WITH_FMT(x, fmt)    telnetClient.print(x, fmt)
-#define     DEBUG_PRINTLN(x)                telnetClient.println(x)
-#define     DEBUG_PRINTLN_WITH_FMT(x, fmt)  telnetClient.println(x, fmt)
-#else
-#define     DEBUG_PRINT(x)                  Serial.print(x)
-#define     DEBUG_PRINTF(x,y)               Serial.printf(x,y)
-#define     DEBUG_PRINT_WITH_FMT(x, fmt)    Serial.print(x, fmt)
-#define     DEBUG_PRINTLN(x)                Serial.println(x)
-#define     DEBUG_PRINTLN_WITH_FMT(x, fmt)  Serial.println(x, fmt)
-#endif
+  void handleTelnet(void);
+  
+  // ################# Macros for debugging ################
+  
+  #ifdef DEBUG_TELNET
+    #define     DEBUG_PRINT(x)                  telnetClient.print(x)
+    #define     DEBUG_PRINTF(x,y)               telnetClient.printf(x,y)
+    #define     DEBUG_PRINT_WITH_FMT(x, fmt)    telnetClient.print(x, fmt)
+    #define     DEBUG_PRINTLN(x)                telnetClient.println(x)
+    #define     DEBUG_PRINTLN_WITH_FMT(x, fmt)  telnetClient.println(x, fmt)
+  #else
+    #define     DEBUG_PRINT(x)                  Serial.print(x)
+    #define     DEBUG_PRINTF(x,y)               Serial.printf(x,y)
+    #define     DEBUG_PRINT_WITH_FMT(x, fmt)    Serial.print(x, fmt)
+    #define     DEBUG_PRINTLN(x)                Serial.println(x)
+    #define     DEBUG_PRINTLN_WITH_FMT(x, fmt)  Serial.println(x, fmt)
+  #endif
 
 #endif