diff --git a/Firmware/EggbotWireless/src/Config.cpp b/Firmware/EggbotWireless/src/Config.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/Firmware/EggbotWireless/src/GCodeParser.cpp b/Firmware/EggbotWireless/src/GCodeParser.cpp
index 08d9657..c09a8a5 100644
--- a/Firmware/EggbotWireless/src/GCodeParser.cpp
+++ b/Firmware/EggbotWireless/src/GCodeParser.cpp
@@ -1,19 +1,19 @@
 #include <Arduino.h>
+#include <unordered_map>
 
 #include "GCodeParser.h"
 
 Command bufcmd;
 
 Command parseGCode(String gcode) {
-    char commandStringIn[50];
     char commandString[50];
+    std::unordered_map<char, float> argsMap;
 
-    gcode.toCharArray(commandStringIn, 50);
+    gcode.toCharArray(commandString, 50);
 
     // Convert command to uppercase
-    for (int i = 0; commandStringIn[i] != '\0'; i++) {
-        commandString[i] = toupper(commandStringIn[i]);
-        commandString[i + 1] = '\0';
+    for (int i = 0; commandString[i] != '\0'; i++) {
+        commandString[i] = toupper(commandString[i]);
     }
 
     char command[4];
@@ -23,42 +23,46 @@ Command parseGCode(String gcode) {
     char args[45];
     strncpy(args, &commandString[4], 45);
 
+    char split_args[6][20];
+    memset(split_args, 0, sizeof(split_args));
+    char* arg;
+    int argc = 0;
+    arg = strtok(args, " ");
+
+    // Put every command separated by spaces into the buffer and count them
+    for (int i = 0; arg != NULL; i++) {
+        strcpy(split_args[i], arg);
+        arg = strtok(NULL, " ");
+        argc++;
+    }
+
+    // Iterate through arguments
+    for (int i = 0; i < argc; i++) {
+        arg = split_args[i];
+        char axis;
+        char value[10];
+
+        axis = arg[0];
+
+        strncpy(value, &arg[1], 10);
+
+        float floatValue;
+        floatValue = atof(value);
+
+        argsMap.emplace(axis, floatValue);
+    }
+
     if (strcmp(command, "G01") == 0 || strcmp(command, "G00") == 0) {
-        char split_args[3][40];
-        memset(split_args, 0, sizeof(split_args));
-        char* arg;
-        int argc = 0;
-        arg = strtok(args, " ");
-
-        // Put every command separated by spaces into the buffer and count them
-        for (int i = 0; arg != NULL; i++) {
-            strcpy(split_args[i], arg);
-            arg = strtok(NULL, " ");
-            argc++;
+        if(argsMap.count('X') > 0) {
+            bufcmd.arg1 = argsMap['X'];
+        }
+        if (argsMap.count('Y') > 0) {
+            bufcmd.arg2 = argsMap['Y'];
+        }
+        if (argsMap.count('Z') > 0) {
+            bufcmd.arg3 = argsMap['Z'];
         }
 
-        // Iterate through arguments
-        for (int i = 0; i < argc; i++) {
-            arg = split_args[i];
-            char axis[2];
-            char value[10];
-
-            strncpy(axis, arg, 1);
-            axis[1] = '\0';
-
-            strncpy(value, &arg[1], 10);
-
-            float floatValue;
-            floatValue = atof(value);
-
-            if (strcmp(axis, "X") == 0) {
-                bufcmd.arg1 = floatValue;
-            } else if (strcmp(axis, "Y") == 0) {
-                bufcmd.arg2 = floatValue;
-            } else if (strcmp(axis, "Z") == 0) {
-                bufcmd.arg3 = floatValue;
-            }
-        }
         if (strcmp(command, "G00") == 0) {
             bufcmd.type = CommandType::G00;
             return bufcmd;