 #endif
     addMotion(x, y, _sys.power, _sys.speed, _sys.acceleration);
 }
-void curveTo(long x0, long y0, long x1, long y1, long x2, long y2, long x3, long y3)
-{
-    double subdiv_step = 1.0 / (NUM_STEPS + 1);
-    double subdiv_step2 = subdiv_step * subdiv_step;
-    double subdiv_step3 = subdiv_step * subdiv_step * subdiv_step;
-    double pre1 = 3.0 * subdiv_step;
-    double pre2 = 3.0 * subdiv_step2;
-    double pre4 = 6.0 * subdiv_step2;
-    double pre5 = 6.0 * subdiv_step3;
-    double tmp1x = x0 - x1 * 2.0 + x2;
-    double tmp1y = y0 - y1 * 2.0 + y2;
-    double tmp2x = (x1 - x2) * 3.0 - x0 + x3;
-    double tmp2y = (y1 - y2) * 3.0 - y0 + y3;
-    double fx = x0;
-    double fy = y0;
-    double dfx = (x1 - x0) * pre1 + tmp1x * pre2 + tmp2x * subdiv_step3;
-    double dfy = (y1 - y0) * pre1 + tmp1y * pre2 + tmp2y * subdiv_step3;
-    double ddfx = tmp1x * pre4 + tmp2x * pre5;
-    double ddfy = tmp1y * pre4 + tmp2y * pre5;
-    double dddfx = tmp2x * pre5;
-    double dddfy = tmp2y * pre5;
-    int step = NUM_STEPS;
-#ifdef DEBUG
-    String str = "curve:";
-    str += x0;
-    str += ",";
-    str += y0;
-    str += " ";
-    str += x1;
-    str += ",";
-    str += y1;
-    str += " ";
-    str += x2;
-    str += ",";
-    str += y2;
-    str += " ";
-    str += x3;
-    str += ",";
-    str += y3;
-    Serial.println(str);
-#endif
-    while (step--)
-    {
-        fx += dfx;
-        fy += dfy;
-        dfx += ddfx;
-        dfy += ddfy;
-        ddfx += dddfx;
-        ddfy += dddfy;
-        lineTo(floorf(fx * 100) / 100, floorf(fy * 100) / 100);
-    }
-    lineTo(floorf(x3 * 100) / 100, floorf(y3 * 100) / 100);
-}
-void quadTo(long x0, long y0, long x1, long y1, long x2, long y2)
-{
-#ifdef DEBUG
-    String str = "quad:";
-    str += " ";
-    str += x1;
-    str += ",";
-    str += y1;
-    str += " ";
-    str += x2;
-    str += ",";
-    str += y2;
-    Serial.println(str);
-#endif
-    int step = NUM_STEPS;
-    double fx, fy;
-    for (int i = 0; i < step; i++)
-    {
-        float t = (float)i / (float)step;
-        float u = 1.0 - t;
-        fx = x0 * u * u + x1 * 2 * u * t + x2 * t * t;
-        fy = y0 * u * u + y1 * 2 * u * t + y2 * t * t;
-        lineTo(floorf(fx * 100) / 100, floorf(fy * 100) / 100);
-    }
-    lineTo(floorf(x2 * 100) / 100, floorf(y2 * 100) / 100);
-}
-
-void arc(long sx, long sy, long cx, long cy, long ex, long ey, bool ccw)
-{
-    double angleA, angleB, angle, radius, length, aX, aY, bX, bY;
-    aX = (sx - cx);
-    aY = (sy - cy);
-    bX = (ex - cx);
-    bY = (ey - cy);
-    if (ccw)
-    {
-        angleA = atan2(aY, aX);
-        angleB = atan2(bY, bX);
-    }
-    else
-    {
-        angleA = atan2(bY, bX);
-        angleB = atan2(aY, aX);
-    }
-    if (angleB <= angleA)
-    {
-        angleB += 2 * M_PI;
-    }
-    angle = angleB - angleA;
-    radius = sqrt(aX * aX + aY * aY);
-    length = radius * angle;
-    int steps, s, step;
-    steps = (int)ceil(length / CURVE_SECTION);
-    for (s = 1; s <= steps; s++)
-    {
-        step = (ccw) ? s : (steps - s);
-        float t = step / steps;
-        lineTo(cx + radius * cos(angleA + angle * t), cy + radius * sin(angleA + angle * t));
-    }
+
+bool hasCommand(char cmd)
+{
+    int index = 0;
+    char c = 0;
+    while (c != cmd)
+    {
+        if (index >= _sys.buffer.length())
+        {
+            return false;
+        }
+        else
+        {
+            c = _sys.buffer.charAt(index);
+        }
+        index++;
+    }
+    return true;
+}
+double getCommand(char cmd)
+{
+    int index = _sys.buffer.indexOf(cmd);
+    int startIndex = _sys.buffer.indexOf(cmd) + 1;
+    char c = 0;
+    while (c != ' ')
+    {
+        index++;
+        if (index >= _sys.buffer.length())
+        {
+            break;
+        }
+        else
+        {
+            c = _sys.buffer.charAt(index);
+        }
+    }
+    return _sys.buffer.substring(startIndex, index).toDouble();
 }
#define CBI(n, b) (n &= ~_BV(b))

#define POINTS_COUNT 6
#define PULSES_PER_MM 100
#define CURVE_SECTION 0.5
#define NUM_STEPS 20
#define NUM_AXIS 2

    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;
