diff --git a/main.c b/main.c
index a0fca44..699c66b 100644
--- a/main.c
+++ b/main.c
@@ -16,16 +16,6 @@ int main(int argc, char **argv)
     void *tvBuffer = memalign(0x100, tvBufferSize);
     void *drcBuffer = memalign(0x100, drcBufferSize);
 
-    void quit(void)
-    {
-        if (tvBuffer)
-            free(tvBuffer);
-        if (drcBuffer)
-            free(drcBuffer);
-        OSScreenShutdown();
-        WHBProcShutdown();
-    }
-
     if (!tvBuffer || !drcBuffer)
     {
         quit();
@@ -48,13 +38,37 @@ int main(int argc, char **argv)
     OSScreenPutFontEx(SCREEN_TV, 0, 0, "SimpleDrawU (TV).");
     OSScreenPutFontEx(SCREEN_DRC, 0, 0, "SimpleDrawU (Gamepad).");
 
-    enum penColor {
-        white  = 0xFFFFFF00,
-        black  = 0x00000000,
-        red    = 0xBF5B5B00,
+    enum penColor
+    {
+        white = 0xFFFFFF00,
+        black = 0x00000000,
+        red = 0xBF5B5B00,
         yellow = 0xC6B95500,
-        blue   = 0x3C8D8800,
-        green  = 0x86B46000,
+        blue = 0x3C8D8800,
+        green = 0x86B46000,
+    };
+
+    void quit(void)
+    {
+        if (tvBuffer)
+            free(tvBuffer);
+        if (drcBuffer)
+            free(drcBuffer);
+        OSScreenShutdown();
+        WHBProcShutdown();
+    };
+
+    void drawScreen(OSScreenID screen, VPADTouchData gamepad, int color)
+    {
+        OSScreenPutPixelEx(screen, gamepad.x - 1, gamepad.y - 1, color);
+        OSScreenPutPixelEx(screen, gamepad.x + 1, gamepad.y + 1, color);
+        OSScreenPutPixelEx(screen, gamepad.x + 1, gamepad.y, color);
+        OSScreenPutPixelEx(screen, gamepad.x - 1, gamepad.y, color);
+        OSScreenPutPixelEx(screen, gamepad.x, gamepad.y, color);
+        OSScreenPutPixelEx(screen, gamepad.x, gamepad.y + 1, color);
+        OSScreenPutPixelEx(screen, gamepad.x, gamepad.y - 1, color);
+        OSScreenPutPixelEx(screen, gamepad.x - 1, gamepad.y + 1, color);
+        OSScreenPutPixelEx(screen, gamepad.x + 1, gamepad.y - 1, color);
     };
 
     while (WHBProcIsRunning())
@@ -121,17 +135,8 @@ int main(int argc, char **argv)
         // draw on screen
         if (gamepadTouchScreen.touched)
         {
-            OSScreenPutPixelEx(SCREEN_TV, gamepadTouchScreen.x - 1, gamepadTouchScreen.y - 1, penColor);
-            OSScreenPutPixelEx(SCREEN_TV, gamepadTouchScreen.x + 1, gamepadTouchScreen.y + 1, penColor);
-            OSScreenPutPixelEx(SCREEN_TV, gamepadTouchScreen.x, gamepadTouchScreen.y, penColor);
-            OSScreenPutPixelEx(SCREEN_TV, gamepadTouchScreen.x - 1, gamepadTouchScreen.y + 1, penColor);
-            OSScreenPutPixelEx(SCREEN_TV, gamepadTouchScreen.x + 1, gamepadTouchScreen.y - 1, penColor);
-
-            OSScreenPutPixelEx(SCREEN_DRC, gamepadTouchScreen.x + 1, gamepadTouchScreen.y + 1, penColor);
-            OSScreenPutPixelEx(SCREEN_DRC, gamepadTouchScreen.x - 1, gamepadTouchScreen.y - 1, penColor);
-            OSScreenPutPixelEx(SCREEN_DRC, gamepadTouchScreen.x, gamepadTouchScreen.y, penColor);
-            OSScreenPutPixelEx(SCREEN_DRC, gamepadTouchScreen.x - 1, gamepadTouchScreen.y + 1, penColor);
-            OSScreenPutPixelEx(SCREEN_DRC, gamepadTouchScreen.x + 1, gamepadTouchScreen.y - 1, penColor);
+            drawScreen(SCREEN_TV, gamepadTouchScreen, penColor);
+            drawScreen(SCREEN_DRC, gamepadTouchScreen, penColor);
         }
 
         DCFlushRange(tvBuffer, tvBufferSize);