     }
 }
 
-static String vStr = "";
-static double v[8];
-void parseCommand(String cmd)
-{
-    cmd.toLowerCase();
-    char code = cmd.charAt(0);
-    long x1, y1, x2, y2, x3, y3, x4, y4;
-    uint8_t vIndex = 0;
-    uint8_t len = cmd.length();
-    vStr = "";
-    for (int i = 1; i < len; i++)
-    {
-        char c = cmd.charAt(i);
-        if (c == ' ' || c == ',')
-        {
-            if (vIndex > 0)
-            {
-                v[vIndex - 1] = vStr.toDouble();
-            }
-            vStr = "";
-            vIndex++;
-        }
-        else
-        {
-            vStr += c;
-            if (vIndex > 0 && i == len - 1)
-            {
-                v[vIndex - 1] = vStr.toDouble();
-            }
-        }
-    }
-    switch (code)
-    {
-    case 'm':
-    {
-        x1 = v[0] * PULSES_PER_MM;
-        y1 = v[1] * PULSES_PER_MM;
-        moveTo(x1, y1);
-        _sys.lastPosition[0] = x1;
-        _sys.lastPosition[1] = y1;
-        break;
-    }
-    case 'l':
-    {
-        x1 = v[0] * PULSES_PER_MM;
-        y1 = v[1] * PULSES_PER_MM;
-        lineTo(x1, y1);
-        _sys.lastPosition[0] = x1;
-        _sys.lastPosition[1] = y1;
-        break;
-    }
-    case 'h':
-    {
-        x1 = v[0] * PULSES_PER_MM;
-        lineTo(x1, _sys.lastPosition[1]);
-        _sys.lastPosition[0] = x1;
-        break;
-    }
-    case 'v':
-    {
-        y1 = v[0] * PULSES_PER_MM;
-        lineTo(_sys.lastPosition[0], y1);
-        _sys.lastPosition[1] = y1;
-        break;
-    }
-    case 'c':
-    {
-        x1 = v[0] * PULSES_PER_MM;
-        y1 = v[1] * PULSES_PER_MM;
-        x2 = v[2] * PULSES_PER_MM;
-        y2 = v[3] * PULSES_PER_MM;
-        x3 = v[4] * PULSES_PER_MM;
-        y3 = v[5] * PULSES_PER_MM;
-        curveTo(_sys.lastPosition[0], _sys.lastPosition[1], x1, y1, x2, y2, x3, y3);
-        _sys.lastPosition[0] = x3;
-        _sys.lastPosition[1] = y3;
-        break;
-    }
-    case 'q':
-    {
-        x1 = v[0] * PULSES_PER_MM;
-        y1 = v[1] * PULSES_PER_MM;
-        x2 = v[2] * PULSES_PER_MM;
-        y2 = v[3] * PULSES_PER_MM;
-        quadTo(_sys.lastPosition[0], _sys.lastPosition[1], x1, y1, x2, y2);
-        _sys.lastPosition[0] = x2;
-        _sys.lastPosition[1] = y2;
-        break;
-    }
-    case 'z':
-    {
-        lineTo(_sys.startPosition[0], _sys.startPosition[1]);
-        _sys.lastPosition[0] = _sys.startPosition[0];
-        _sys.lastPosition[1] = _sys.startPosition[1];
-        break;
-    }
-    case 'p':
-    {
-        setPower(v[0]);
-        setSpeed(v[1]);
-        setAccel(v[2]);
-        break;
-    }
-    case 'e':
-    {
-        firePower(v[0]);
-        break;
-    }
-    case 'o':
-    {
-        waitingForFinish();
-        if (v[0] > 0)
-        {
-            for (int i = 0; i < NUM_AXIS; i++)
-            {
-                steppers[i].enableOutputs();
-            }
-        }
-        else
-        {
-            for (int i = 0; i < NUM_AXIS; i++)
-            {
-                steppers[i].disableOutputs();
-            }
-        }
-        break;
-    }
+void parseCommand()
+{
+    _sys.buffer.toLowerCase();
+    int cmdId;
+    if (_sys.buffer.length() > 2)
+    {
+        if (hasCommand('g'))
+        {
+            cmdId = getCommand('g');
+            switch (cmdId)
+            {
+            case 0:
+            case 1:
+            {
+                if (hasCommand('p'))
+                {
+                    firePower(getCommand('p'));
+                }
+                if (hasCommand('f'))
+                {
+                    setSpeed(getCommand('f'));
+                }
+                long x = (hasCommand('x') ? getCommand('x') : 0) * PULSES_PER_MM;
+                long y = (hasCommand('y') ? getCommand('y') : 0) * PULSES_PER_MM;
+                if (_sys.isRelative)
+                {
+                    x += _sys.targetPosition[0];
+                    y += _sys.targetPosition[1];
+                }
+                lineTo(x, y);
+                _sys.targetPosition[0] = x;
+                _sys.targetPosition[1] = y;
+            }
+            break;
+            case 28:
+            {
+                waitingForFinish();
+                _sys.currentPosition[0] = 0;
+                _sys.currentPosition[1] = 0;
+            }
+            break;
+            case 90:
+            {
+                waitingForFinish();
+                _sys.isRelative = false;
+            }
+            break;
+            case 91:
+            {
+                waitingForFinish();
+                _sys.isRelative = true;
+            }
+            break;
+            }
+        }
+        else if (hasCommand('m'))
+        {
+            cmdId = getCommand('m');
+            switch (cmdId)
+            {
+            case 1:
+            {
+                waitingForFinish();
+                if (hasCommand('p') && getCommand('p') == 1)
+                {
+                    for (int i = 0; i < NUM_AXIS; i++)
+                    {
+                        steppers[i].enableOutputs();
+                    }
+                }
+                else
+                {
+                    for (int i = 0; i < NUM_AXIS; i++)
+                    {
+                        steppers[i].disableOutputs();
+                    }
+                }
+                break;
+            }
+            case 4:
+            {
+                waitingForFinish();
+                if (hasCommand('p'))
+                {
+                    firePower(getCommand('p'));
+                }
+                else
+                {
+                    firePower(0);
+                }
+                break;
+            }
+            }
+        }
     }
     finishCommand();
 }

#endif
    _sys.startPosition[0] = x;
    _sys.startPosition[1] = y;
    _sys.currentSpeed = _sys.defaultSpeed;
    addMotion(x, y, 0, _sys.speed, _sys.acceleration);
}
void lineTo(long x, long y)
{

    volatile uint8_t plannedLength = 0;
    volatile uint8_t plannedIndex = 0;
    volatile uint8_t isRunningState = MOTION_ACCELETING;
    volatile bool isRelative = false;
    uint8_t powerPin = 10;
    String buffer = "";
} SystemStatus;
