 {
     while (true)
     {
-        if (_sys.statesLength < STATES_COUNT)
+        if (_sys.plannedLength < POINTS_COUNT)
         {
             break;
         }
         delay(200);
 #ifdef DEBUG
         String str = "waiting:";
-        str += _sys.statesIndex;
+        str += _sys.plannedLength;
         Serial.println(str);
 #endif
     }

/**
 * motion control in Timer
 */
void shiftState()
{
    _sys.state = _sys.states[0];
    for (int i = 0; i < _sys.statesLength - 1; i++)
    {
        _sys.states[i] = _sys.states[i + 1];
    }
    _sys.statesIndex--;
    _sys.statesLength--;
    if (_sys.statesLength <= 0)
    {
        finishCommand();
    }
}
void calcPlanned()
{
    if (_sys.plannedLength < POINTS_COUNT)
    {
        if (_sys.statesLength > 1)
        {
            _sys.allowEnding = true;
            MotionState currentState = _sys.state;
            shiftState();
            MotionState nextState = _sys.state;
            PlannerPoint point;
            point.dir[0] = currentState.targetPosition[0] < nextState.targetPosition[0];
            point.dir[1] = currentState.targetPosition[1] < nextState.targetPosition[1];
            point.targetCount[0] = abs(nextState.targetPosition[0] - currentState.targetPosition[0]);
            point.targetCount[1] = abs(nextState.targetPosition[1] - currentState.targetPosition[1]);
            point.delta[0] = (point.targetCount[0]);
            point.delta[1] = (point.targetCount[1]);
            point.totalCount = point.delta[0] + point.delta[1];
            point.enterSpeed = _sys.plannedIndex > 0 ? _sys.points[_sys.plannedIndex - 1].exitSpeed : _sys.defaultSpeed;
            point.speed = fmax(1, currentState.speed);
            point.exitSpeed = fmax(1, sqrt(nextState.speed));
            point.acceleration = currentState.acceleration;
            long dist = sqrt(point.delta[0] * point.delta[0] + point.delta[1] * point.delta[1]) / 2;
            point.accelCount = point.totalCount - min(dist, _sys.defaultAccelCount);
            point.decelCount = min(dist, _sys.defaultDecelCount);
            point.power = currentState.power;
            point.err = (point.delta[0] > point.delta[1] ? point.delta[0] : -point.delta[1]) / 2;
            _sys.points[_sys.plannedIndex] = point;
            _sys.plannedIndex++;
            _sys.plannedLength++;
        }
        else
        {
        }
    }
}
/**
 * string -> command list -> line list -> point list
 * line list < 2 wait - stopping
 * line list >=2 calc point list - running
 * line list >=6 wait - running
 * */
void shiftPlanned()
{
    if (_sys.plannedLength > 0)

    finishCommand();
}

void addMotion(long x, long y, uint8_t power, uint8_t speed, uint8_t acceleration)
{
    if (_sys.plannedLength >= POINTS_COUNT)
    {
        waitingPushPoint();
    }
    PlannerPoint prevPoint = _sys.prevPoint;
    PlannerPoint nextPoint;
    nextPoint.targetPosition[0] = x;
    nextPoint.targetPosition[1] = y;
    nextPoint.dir[0] = prevPoint.targetPosition[0] < nextPoint.targetPosition[0];
    nextPoint.dir[1] = prevPoint.targetPosition[1] < nextPoint.targetPosition[1];
    nextPoint.targetCount[0] = abs(nextPoint.targetPosition[0] - prevPoint.targetPosition[0]);
    nextPoint.targetCount[1] = abs(nextPoint.targetPosition[1] - prevPoint.targetPosition[1]);
    nextPoint.delta[0] = (nextPoint.targetCount[0]);
    nextPoint.delta[1] = (nextPoint.targetCount[1]);
    nextPoint.totalCount = nextPoint.delta[0] + nextPoint.delta[1];
    nextPoint.enterSpeed = _sys.plannedIndex > 0 ? prevPoint.exitSpeed : _sys.defaultSpeed;
    nextPoint.speed = fmax(1, speed);
    prevPoint.exitSpeed = fmax(1, sqrt(nextPoint.speed));
    nextPoint.acceleration = acceleration;
    long dist = sqrt(nextPoint.delta[0] * nextPoint.delta[0] + nextPoint.delta[1] * nextPoint.delta[1]) / 2;
    nextPoint.accelCount = nextPoint.totalCount - min(dist, _sys.defaultAccelCount);
    nextPoint.decelCount = min(dist, _sys.defaultDecelCount);
    nextPoint.power = power;
    nextPoint.err = (nextPoint.delta[0] > nextPoint.delta[1] ? nextPoint.delta[0] : -nextPoint.delta[1]) / 2;
    _sys.points[_sys.plannedIndex] = nextPoint;
    _sys.prevPoint = nextPoint;
    _sys.plannedIndex++;
    _sys.plannedLength++;
}
void finishCommand()
{
    if (commandsLength() < POINTS_COUNT)
    {
        Serial.print("ok:");
        Serial.println(commandsLength());
