motion.cpp

/*
    This file is part of Repetier-Firmware.

    Repetier-Firmware is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Repetier-Firmware is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Repetier-Firmware.  If not, see <http://www.gnu.org/licenses/>.

    This firmware is a nearly complete rewrite of the sprinter firmware
    by kliment (https://github.com/kliment/Sprinter)
    which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.

  Functions in this file are used to communicate using ascii or repetier protocol.
*/

#include "Repetier.h"

// ================ Sanity checks ================
#ifndef STEP_DOUBLER_FREQUENCY
#error Please add new parameter STEP_DOUBLER_FREQUENCY to your configuration.
#else
#if STEP_DOUBLER_FREQUENCY < 7000 || STEP_DOUBLER_FREQUENCY > 20000
#if CPU_ARCH==ARCH_AVR
#error STEP_DOUBLER_FREQUENCY should be in range 10000-16000.
#endif
#endif
#endif
#ifdef EXTRUDER_SPEED
#error EXTRUDER_SPEED is not used any more. Values are now taken from extruder definition.
#endif
#ifdef ENDSTOPPULLUPS
#error ENDSTOPPULLUPS is now replaced by individual pullup configuration!
#endif
#ifdef EXT0_PID_PGAIN
#error The PID system has changed. Please use the new float number options!
#endif
// ####################################################################################
// #          No configuration below this line - just some error checking             #
// ####################################################################################
#ifdef SUPPORT_MAX6675
#if !defined SCK_PIN || !defined MOSI_PIN || !defined MISO_PIN
#error For MAX6675 support, you need to define SCK_PIN, MISO_PIN and MOSI_PIN in pins.h
#endif
#endif
#if X_STEP_PIN < 0 || Y_STEP_PIN < 0 || Z_STEP_PIN < 0
#error One of the following pins is not assigned: X_STEP_PIN,Y_STEP_PIN,Z_STEP_PIN
#endif
#if EXT0_STEP_PIN < 0 && NUM_EXTRUDER > 0
#error EXT0_STEP_PIN not set to a pin number.
#endif
#if EXT0_DIR_PIN < 0 && NUM_EXTRUDER > 0
#error EXT0_DIR_PIN not set to a pin number.
#endif
#if PRINTLINE_CACHE_SIZE < 4
#error PRINTLINE_CACHE_SIZE must be at least 5
#endif

//Inactivity shutdown variables
millis_t previousMillisCmd = 0;
millis_t maxInactiveTime = MAX_INACTIVE_TIME * 1000L;
millis_t stepperInactiveTime = STEPPER_INACTIVE_TIME * 1000L;
long baudrate = BAUDRATE;         ///< Communication speed rate.
#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
int maxadv = 0;
#endif
int maxadv2 = 0;
float maxadvspeed = 0;
#endif
uint8_t pwm_pos[NUM_PWM]; // 0-NUM_EXTRUDER = Heater 0-NUM_EXTRUDER of extruder, NUM_EXTRUDER = Heated bed, NUM_EXTRUDER+1 Board fan, NUM_EXTRUDER+2 = Fan
volatile int waitRelax = 0; // Delay filament relax at the end of print, could be a simple timeout

PrintLine PrintLine::lines[PRINTLINE_CACHE_SIZE]; ///< Cache for print moves.
PrintLine *PrintLine::cur = NULL;               ///< Current printing line
#if CPU_ARCH == ARCH_ARM
volatile bool PrintLine::nlFlag = false;
#endif
ufast8_t PrintLine::linesWritePos = 0;            ///< Position where we write the next cached line move.
volatile ufast8_t PrintLine::linesCount = 0;      ///< Number of lines cached 0 = nothing to do.
ufast8_t PrintLine::linesPos = 0;                 ///< Position for executing line movement.

/**
Move printer the given number of steps. Puts the move into the queue. Used by e.g. homing commands.
Does not consider rotation but updates position correctly considering rotation. This can be used to
correct positions when changing tools.

\param x Distance in x direction in steps
\param y Distance in y direction in steps
\param z Distance in z direction in steps
\param e Distance in e direction in steps
\param feedrate Feed rate to be used in mm/s. Gets new active feedrate.
\param waitEnd If true will block until move is finished.
\param checkEndstop True if triggering endstop should stop move.
\param pathOptimize If false start and end speeds get fixed to minimum values.
*/
void PrintLine::moveRelativeDistanceInSteps(int32_t x, int32_t y, int32_t z, int32_t e, float feedrate, bool waitEnd, bool checkEndstop, bool pathOptimize) {
#if NUM_EXTRUDER > 0
    if(Printer::debugDryrun() || (MIN_EXTRUDER_TEMP > 30 && Extruder::current->tempControl.currentTemperatureC < MIN_EXTRUDER_TEMP && !Printer::isColdExtrusionAllowed() && Extruder::current->tempControl.sensorType != 0))
        e = 0; // should not be allowed for current temperature
#endif
#if MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 || MOVE_Z_WHEN_HOMED == 1
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) {
#if MOVE_X_WHEN_HOMED
        if(!Printer::isXHomed())
            x = 0;
#endif
#if MOVE_Y_WHEN_HOMED
        if(!Printer::isYHomed())
            y = 0;
#endif
#if MOVE_Z_WHEN_HOMED
        if(!Printer::isZHomed() && !Printer::isZProbingActive())
            z = 0;
#endif
    }
#endif //  MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 || MOVE_Z_WHEN_HOMED == 1
    float savedFeedrate = Printer::feedrate;
    Printer::destinationSteps[X_AXIS] = Printer::currentPositionSteps[X_AXIS] + x;
    Printer::destinationSteps[Y_AXIS] = Printer::currentPositionSteps[Y_AXIS] + y;
    Printer::destinationSteps[Z_AXIS] = Printer::currentPositionSteps[Z_AXIS] + z;
    Printer::destinationSteps[E_AXIS] = Printer::currentPositionSteps[E_AXIS] + e;
    Printer::feedrate = feedrate;
#if NONLINEAR_SYSTEM
    if (!queueNonlinearMove(checkEndstop, pathOptimize, false)) {
        Com::printWarningFLN(PSTR("moveRelativeDistanceInSteps / queueDeltaMove returns error"));
    }
#else
#if DISTORTION_CORRECTION
    Printer::destinationSteps[Z_AXIS] -= Printer::zCorrectionStepsIncluded; // correct as it will be added later in Cartesian move computation
#endif
    queueCartesianMove(checkEndstop, pathOptimize);
#endif
    Printer::feedrate = savedFeedrate;
    Printer::updateCurrentPosition(false);
    if(waitEnd)
        Commands::waitUntilEndOfAllMoves();
    previousMillisCmd = HAL::timeInMilliseconds();
}

/** Adds the steps converted to mm to the lastCmdPos position and moves to that position using Printer::moveToReal.
Will use Printer::isPositionAllowed to prevent illegal moves.

\param x Distance in x direction in steps
\param y Distance in y direction in steps
\param z Distance in z direction in steps
\param e Distance in e direction in steps
\param feedrate Feed rate to be used in mm/s. Gets new active feedrate.
\param waitEnd If true will block until move is finished.
\param pathOptimize If false start and end speeds get fixed to minimum values.
*/
void PrintLine::moveRelativeDistanceInStepsReal(int32_t x, int32_t y, int32_t z, int32_t e, float feedrate, bool waitEnd, bool pathOptimize) {
#if MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 || MOVE_Z_WHEN_HOMED == 1
    if(!Printer::isHoming() && !Printer::isNoDestinationCheck()) { // prevent movements when not homed
#if MOVE_X_WHEN_HOMED
        if(!Printer::isXHomed())
            x = 0;
#endif
#if MOVE_Y_WHEN_HOMED
        if(!Printer::isYHomed())
            y = 0;
#endif
#if MOVE_Z_WHEN_HOMED
        if(!Printer::isZHomed() && !Printer::isZProbingActive())
            z = 0;
#endif
    }
#endif // MOVE_X_WHEN_HOMED == 1 || MOVE_Y_WHEN_HOMED == 1 || MOVE_Z_WHEN_HOMED == 1
    Printer::lastCmdPos[X_AXIS] += x * Printer::invAxisStepsPerMM[X_AXIS];
    Printer::lastCmdPos[Y_AXIS] += y * Printer::invAxisStepsPerMM[Y_AXIS];
    Printer::lastCmdPos[Z_AXIS] += z * Printer::invAxisStepsPerMM[Z_AXIS];
#if LAZY_DUAL_X_AXIS
    Printer::sledParked = false;
#endif

    if(!Printer::isPositionAllowed( Printer::lastCmdPos[X_AXIS], Printer::lastCmdPos[Y_AXIS], Printer::lastCmdPos[Z_AXIS])) {
        return; // ignore move
    }
#if NUM_EXTRUDER > 0
    if(Printer::debugDryrun() || (MIN_EXTRUDER_TEMP > 30 && Extruder::current->tempControl.currentTemperatureC < MIN_EXTRUDER_TEMP && !Printer::isColdExtrusionAllowed() && Extruder::current->tempControl.sensorType != 0))
        e = 0; // should not be allowed for current temperature
#endif
    Printer::moveToReal(Printer::lastCmdPos[X_AXIS], Printer::lastCmdPos[Y_AXIS], Printer::lastCmdPos[Z_AXIS],
                        (Printer::currentPositionSteps[E_AXIS] + e) * Printer::invAxisStepsPerMM[E_AXIS], feedrate, pathOptimize);
    Printer::updateCurrentPosition();
    if(waitEnd)
        Commands::waitUntilEndOfAllMoves();
    previousMillisCmd = HAL::timeInMilliseconds();
}

#if !NONLINEAR_SYSTEM
#if DISTORTION_CORRECTION
/* Special version which adds distortion correction to z. Gets called from queueCartesianMove if needed. */
void PrintLine::queueCartesianSegmentTo(uint8_t check_endstops, uint8_t pathOptimize) {

    // Correct the bumps
    Printer::zCorrectionStepsIncluded = Printer::distortion.correct(Printer::destinationSteps[X_AXIS], Printer::destinationSteps[Y_AXIS], Printer::destinationSteps[Z_AXIS]);
    Printer::destinationSteps[Z_AXIS] += Printer::zCorrectionStepsIncluded;
#if DEBUG_DISTORTION
    Com::printF(PSTR("zCorr:"), Printer::zCorrectionStepsIncluded * Printer::invAxisStepsPerMM[Z_AXIS], 3);
    Com::printF(PSTR(" atX:"), Printer::destinationSteps[X_AXIS]*Printer::invAxisStepsPerMM[X_AXIS]);
    Com::printFLN(PSTR(" atY:"), Printer::destinationSteps[Y_AXIS]*Printer::invAxisStepsPerMM[Y_AXIS]);
#endif
    PrintLine::waitForXFreeLines(1);
    uint8_t newPath = PrintLine::insertWaitMovesIfNeeded(pathOptimize, 0);
    PrintLine *p = PrintLine::getNextWriteLine();

    float axisDistanceMM[E_AXIS_ARRAY]; // Axis movement in mm
    p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
#if MIXING_EXTRUDER
    if(Printer::isAllEMotors()) {
        p->flags |= FLAG_ALL_E_MOTORS;
    }
#endif
    p->joinFlags = 0;
    if(!pathOptimize) p->setEndSpeedFixed(true);
    p->dir = 0;
    //Find direction
    //Printer::zCorrectionStepsIncluded = 0;
    for(uint8_t axis = 0; axis < 4; axis++) {
        p->delta[axis] = Printer::destinationSteps[axis] - Printer::currentPositionSteps[axis];
        p->secondSpeed = Printer::fanSpeed;
        if(axis == E_AXIS) {
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::extrudeMultiplyError += (static_cast<float>(p->delta[E_AXIS]) * Printer::extrusionFactor);
                p->delta[E_AXIS] = static_cast<int32_t>(Printer::extrudeMultiplyError);
                Printer::extrudeMultiplyError -= p->delta[E_AXIS];
                Printer::filamentPrinted += p->delta[E_AXIS] * Printer::invAxisStepsPerMM[axis];
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) {
                p->secondSpeed = ((p->delta[X_AXIS] != 0 || p->delta[Y_AXIS] != 0) && (LaserDriver::laserOn || p->delta[E_AXIS] != 0) ? LaserDriver::intensity : 0);
                p->delta[E_AXIS] = 0;
            }
#endif
        }
        if(p->delta[axis] >= 0)
            p->setPositiveDirectionForAxis(axis);
        else
            p->delta[axis] = -p->delta[axis];
        axisDistanceMM[axis] = p->delta[axis] * Printer::invAxisStepsPerMM[axis];
        if(p->delta[axis]) p->setMoveOfAxis(axis);
        Printer::currentPositionSteps[axis] = Printer::destinationSteps[axis];
    }
    if(p->isNoMove()) {
        if(newPath)   // need to delete dummy elements, otherwise commands can get locked.
            PrintLine::resetPathPlanner();
        return; // No steps included
    }
    float xydist2;
#if ENABLE_BACKLASH_COMPENSATION
    if((p->isXYZMove()) && ((p->dir & XYZ_DIRPOS) ^ (Printer::backlashDir & XYZ_DIRPOS)) & (Printer::backlashDir >> 3)) { // We need to compensate backlash, add a move
        PrintLine::waitForXFreeLines(2);
        uint8_t wpos2 = PrintLine::linesWritePos + 1;
        if(wpos2 >= PRINTLINE_CACHE_SIZE) wpos2 = 0;
        PrintLine *p2 = &PrintLine::lines[wpos2];
        memcpy(p2, p, sizeof(PrintLine)); // Move current data to p2
        uint8_t changed = (p->dir & XYZ_DIRPOS) ^ (Printer::backlashDir & XYZ_DIRPOS);
        float back_diff[4]; // Axis movement in mm
        back_diff[E_AXIS] = 0;
        back_diff[X_AXIS] = (changed & 1 ? (p->isXPositiveMove() ? Printer::backlashX : -Printer::backlashX) : 0);
        back_diff[Y_AXIS] = (changed & 2 ? (p->isYPositiveMove() ? Printer::backlashY : -Printer::backlashY) : 0);
        back_diff[Z_AXIS] = (changed & 4 ? (p->isZPositiveMove() ? Printer::backlashZ : -Printer::backlashZ) : 0);
        p->dir &= XYZ_DIRPOS; // x,y and z are already correct
        for(uint8_t i = 0; i < 4; i++) {
            float f = back_diff[i] * Printer::axisStepsPerMM[i];
            p->delta[i] = abs((long)f);
            if(p->delta[i]) p->dir |= XSTEP << i;
        }
        //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
        if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS]) p->primaryAxis = Y_AXIS;
        else if (p->delta[X_AXIS] > p->delta[Z_AXIS] ) p->primaryAxis = X_AXIS;
        else p->primaryAxis = Z_AXIS;
        p->stepsRemaining = p->delta[p->primaryAxis];
        //Feedrate calc based on XYZ travel distance
        xydist2 = back_diff[X_AXIS] * back_diff[X_AXIS] + back_diff[Y_AXIS] * back_diff[Y_AXIS];
        if(p->isZMove())
            p->distance = sqrt(xydist2 + back_diff[Z_AXIS] * back_diff[Z_AXIS]);
        else
            p->distance = sqrt(xydist2);
        Printer::backlashDir = (Printer::backlashDir & 56) | (p2->dir & XYZ_DIRPOS);
        p->calculateMove(back_diff, pathOptimize, p->primaryAxis);
        p = p2; // use saved instance for the real move
    }
#endif

    //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
    if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS] && p->delta[Y_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Y_AXIS;
    else if (p->delta[X_AXIS] > p->delta[Z_AXIS] && p->delta[X_AXIS] > p->delta[E_AXIS]) p->primaryAxis = X_AXIS;
    else if (p->delta[Z_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Z_AXIS;
    else p->primaryAxis = E_AXIS;
    p->stepsRemaining = p->delta[p->primaryAxis];
    if(p->isXYZMove()) {
        xydist2 = axisDistanceMM[X_AXIS] * axisDistanceMM[X_AXIS] + axisDistanceMM[Y_AXIS] * axisDistanceMM[Y_AXIS];
        if(p->isZMove())
            p->distance = RMath::max((float)sqrt(xydist2 + axisDistanceMM[Z_AXIS] * axisDistanceMM[Z_AXIS]), fabs(axisDistanceMM[E_AXIS]));
        else
            p->distance = RMath::max((float)sqrt(xydist2), fabs(axisDistanceMM[E_AXIS]));
    } else
        p->distance = fabs(axisDistanceMM[E_AXIS]);
    p->calculateMove(axisDistanceMM, pathOptimize, p->primaryAxis);

}
#endif
/**
  Put a move to the current destination coordinates into the movement cache.
  If the cache is full, the method will wait, until a place gets free. During
  wait communication and temperature control is enabled.

  destinationSteps must be excluding any z correction! We will add that if required here.

  @param check_endstops Read end stop during move.
*/
void PrintLine::queueCartesianMove(uint8_t check_endstops, uint8_t pathOptimize) {
	ENSURE_POWER
#if LAZY_DUAL_X_AXIS
    if(Printer::sledParked && (Printer::currentPositionSteps[X_AXIS] != Printer::destinationSteps[X_AXIS] ||
                               Printer::currentPositionSteps[Y_AXIS] != Printer::destinationSteps[Y_AXIS] ||
                               Printer::currentPositionSteps[Z_AXIS] != Printer::destinationSteps[Z_AXIS]))
        Printer::sledParked = false;
#endif
    Printer::constrainDestinationCoords();
    Printer::unsetAllSteppersDisabled();
#if DISTORTION_CORRECTION
    if(Printer::distortion.isEnabled() && Printer::destinationSteps[Z_AXIS] < Printer::distortion.zMaxSteps() && Printer::isZProbingActive() == false && !Printer::isHoming()) {
        // we are inside correction height so we split all moves in lines of max. 10 mm and add them
        // including a z correction
        int32_t deltas[E_AXIS_ARRAY], start[E_AXIS_ARRAY];
        for(fast8_t i = 0; i < E_AXIS_ARRAY; i++) {
            deltas[i] = Printer::destinationSteps[i] - Printer::currentPositionSteps[i];
            start[i] = Printer::currentPositionSteps[i];
        }
        deltas[Z_AXIS] += Printer::zCorrectionStepsIncluded;
        start[Z_AXIS] -= Printer::zCorrectionStepsIncluded;
        float dx = Printer::invAxisStepsPerMM[X_AXIS] * deltas[X_AXIS];
        float dy = Printer::invAxisStepsPerMM[Y_AXIS] * deltas[Y_AXIS];
        float len = dx * dx + dy * dy;
        if(len < 100) { // no splitting required
            queueCartesianSegmentTo(check_endstops, pathOptimize);
            return;
        }
        // we need to split longer lines to follow bed curvature
        len = sqrt(len);
        int segments = (static_cast<int>(len) + 9) / 10;
#if DEBUG_DISTORTION
        Com::printF(PSTR("Split line len:"), len);
        Com::printFLN(PSTR(" segments:"), segments);
#endif
        for(int i = 1; i <= segments; i++) {
            for(fast8_t j = 0; j < E_AXIS_ARRAY; j++) {
                Printer::destinationSteps[j] = start[j] + (i * deltas[j]) / segments;
            }
            queueCartesianSegmentTo(check_endstops, pathOptimize);
        }
        return;
    }
#endif
    waitForXFreeLines(1);
    uint8_t newPath = insertWaitMovesIfNeeded(pathOptimize, 0);
    PrintLine *p = getNextWriteLine();

    float axisDistanceMM[E_AXIS_ARRAY]; // Axis movement in mm
    p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
#if MIXING_EXTRUDER
    if(Printer::isAllEMotors()) {
        p->flags |= FLAG_ALL_E_MOTORS;
    }
#endif
    p->joinFlags = 0;
    if(!pathOptimize) p->setEndSpeedFixed(true);
    p->dir = 0;
    //Find direction
    Printer::zCorrectionStepsIncluded = 0;
    for(uint8_t axis = 0; axis < 4; axis++) {
        p->delta[axis] = Printer::destinationSteps[axis] - Printer::currentPositionSteps[axis];
        p->secondSpeed = Printer::fanSpeed;
        if(axis == E_AXIS) {
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::extrudeMultiplyError += (static_cast<float>(p->delta[E_AXIS]) * Printer::extrusionFactor);
                p->delta[E_AXIS] = static_cast<int32_t>(Printer::extrudeMultiplyError);
                Printer::extrudeMultiplyError -= p->delta[E_AXIS];
                Printer::filamentPrinted += p->delta[E_AXIS] * Printer::invAxisStepsPerMM[axis];
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) {
                p->secondSpeed = ((p->delta[X_AXIS] != 0 || p->delta[Y_AXIS] != 0) && (LaserDriver::laserOn || p->delta[E_AXIS] != 0) ? LaserDriver::intensity : 0);
                p->delta[E_AXIS] = 0;
            }
#endif
        }
        if(p->delta[axis] >= 0)
            p->setPositiveDirectionForAxis(axis);
        else
            p->delta[axis] = -p->delta[axis];
        axisDistanceMM[axis] = p->delta[axis] * Printer::invAxisStepsPerMM[axis];
        if(p->delta[axis]) p->setMoveOfAxis(axis);
        Printer::currentPositionSteps[axis] = Printer::destinationSteps[axis];
    }
    if(p->isNoMove()) {
        if(newPath)   // need to delete dummy elements, otherwise commands can get locked.
            resetPathPlanner();
        return; // No steps included
    }
    float xydist2;
#if ENABLE_BACKLASH_COMPENSATION
    if((p->isXYZMove()) && ((p->dir & XYZ_DIRPOS) ^ (Printer::backlashDir & XYZ_DIRPOS)) & (Printer::backlashDir >> 3)) { // We need to compensate backlash, add a move
        waitForXFreeLines(2);
        uint8_t wpos2 = linesWritePos + 1;
        if(wpos2 >= PRINTLINE_CACHE_SIZE) wpos2 = 0;
        PrintLine *p2 = &lines[wpos2];
        memcpy(p2, p, sizeof(PrintLine)); // Move current data to p2
        uint8_t changed = (p->dir & XYZ_DIRPOS) ^ (Printer::backlashDir & XYZ_DIRPOS);
        float back_diff[4]; // Axis movement in mm
        back_diff[E_AXIS] = 0;
        back_diff[X_AXIS] = (changed & 1 ? (p->isXPositiveMove() ? Printer::backlashX : -Printer::backlashX) : 0);
        back_diff[Y_AXIS] = (changed & 2 ? (p->isYPositiveMove() ? Printer::backlashY : -Printer::backlashY) : 0);
        back_diff[Z_AXIS] = (changed & 4 ? (p->isZPositiveMove() ? Printer::backlashZ : -Printer::backlashZ) : 0);
        p->dir &= XYZ_DIRPOS; // x,y and z are already correct
        for(uint8_t i = 0; i < 4; i++) {
            float f = back_diff[i] * Printer::axisStepsPerMM[i];
            p->delta[i] = abs((long)f);
            if(p->delta[i]) p->dir |= XSTEP << i;
        }
        //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
        if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS]) p->primaryAxis = Y_AXIS;
        else if (p->delta[X_AXIS] > p->delta[Z_AXIS] ) p->primaryAxis = X_AXIS;
        else p->primaryAxis = Z_AXIS;
        p->stepsRemaining = p->delta[p->primaryAxis];
        //Feedrate calc based on XYZ travel distance
        xydist2 = back_diff[X_AXIS] * back_diff[X_AXIS] + back_diff[Y_AXIS] * back_diff[Y_AXIS];
        if(p->isZMove())
            p->distance = sqrt(xydist2 + back_diff[Z_AXIS] * back_diff[Z_AXIS]);
        else
            p->distance = sqrt(xydist2);
        // 56 seems to be xstep|ystep|e_posdir which just seems odd
        Printer::backlashDir = (Printer::backlashDir & 56) | (p2->dir & XYZ_DIRPOS);
        p->calculateMove(back_diff, pathOptimize, p->primaryAxis);
        p = p2; // use saved instance for the real move
    }
#endif

    //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
    if(p->delta[Y_AXIS] > p->delta[X_AXIS] && p->delta[Y_AXIS] > p->delta[Z_AXIS] && p->delta[Y_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Y_AXIS;
    else if (p->delta[X_AXIS] > p->delta[Z_AXIS] && p->delta[X_AXIS] > p->delta[E_AXIS]) p->primaryAxis = X_AXIS;
    else if (p->delta[Z_AXIS] > p->delta[E_AXIS]) p->primaryAxis = Z_AXIS;
    else p->primaryAxis = E_AXIS;
    p->stepsRemaining = p->delta[p->primaryAxis];
    if(p->isXYZMove()) {
        xydist2 = axisDistanceMM[X_AXIS] * axisDistanceMM[X_AXIS] + axisDistanceMM[Y_AXIS] * axisDistanceMM[Y_AXIS];
        if(p->isZMove())
            p->distance = RMath::max((float)sqrt(xydist2 + axisDistanceMM[Z_AXIS] * axisDistanceMM[Z_AXIS]), fabs(axisDistanceMM[E_AXIS]));
        else
            p->distance = RMath::max((float)sqrt(xydist2), fabs(axisDistanceMM[E_AXIS]));
    } else
        p->distance = fabs(axisDistanceMM[E_AXIS]);
    p->calculateMove(axisDistanceMM, pathOptimize, p->primaryAxis);
}
#endif

void PrintLine::calculateMove(float axisDistanceMM[], uint8_t pathOptimize, fast8_t drivingAxis) {
#if NONLINEAR_SYSTEM
    long axisInterval[VIRTUAL_AXIS_ARRAY]; // shortest interval possible for that axis
#else
    long axisInterval[E_AXIS_ARRAY];
#endif
    //float timeForMove = (float)(F_CPU)*distance / (isXOrYMove() ? RMath::max(Printer::minimumSpeed, Printer::feedrate) : Printer::feedrate); // time is in ticks
    float timeForMove = (float)(F_CPU) * distance / Printer::feedrate; // time is in ticks
    //bool critical = Printer::isZProbingActive();
    if(linesCount < MOVE_CACHE_LOW && timeForMove < LOW_TICKS_PER_MOVE) { // Limit speed to keep cache full.
        //Com::printF(PSTR("L:"),(int)linesCount);
        //Com::printF(PSTR(" Old "),timeForMove);
        timeForMove += (3 * (LOW_TICKS_PER_MOVE - timeForMove)) / (linesCount + 1); // Increase time if queue gets empty. Add more time if queue gets smaller.
        //Com::printFLN(PSTR("Slow "),timeForMove);
        //critical = true;
    }
    timeInTicks = timeForMove;
    UI_MEDIUM; // do check encoder
    // Compute the slowest allowed interval (ticks/step), so maximum feedrate is not violated
    int32_t limitInterval0;
    int32_t limitInterval = limitInterval0 = timeForMove / stepsRemaining; // until not violated by other constraints it is your target speed
    float toTicks = static_cast<float>(F_CPU) / stepsRemaining;
    if(isXMove()) {
        axisInterval[X_AXIS] = axisDistanceMM[X_AXIS] * toTicks / (Printer::maxFeedrate[X_AXIS]); // mm*ticks/s/(mm/s*steps) = ticks/step
#if !NONLINEAR_SYSTEM || defined(FAST_COREXYZ)
        limitInterval = RMath::max(axisInterval[X_AXIS], limitInterval);
#endif
    } else axisInterval[X_AXIS] = 0;
    if(isYMove()) {
        axisInterval[Y_AXIS] = axisDistanceMM[Y_AXIS] * toTicks / Printer::maxFeedrate[Y_AXIS];
#if !NONLINEAR_SYSTEM || defined(FAST_COREXYZ)
        limitInterval = RMath::max(axisInterval[Y_AXIS], limitInterval);
#endif
    } else axisInterval[Y_AXIS] = 0;
    if(isZMove()) { // normally no move in z direction
        axisInterval[Z_AXIS] = axisDistanceMM[Z_AXIS] * toTicks / Printer::maxFeedrate[Z_AXIS]; // must prevent overflow!
#if !NONLINEAR_SYSTEM || defined(FAST_COREXYZ)
        limitInterval = RMath::max(axisInterval[Z_AXIS], limitInterval);
#endif
    } else axisInterval[Z_AXIS] = 0;
    if(isEMove()) {
        axisInterval[E_AXIS] = axisDistanceMM[E_AXIS] * toTicks / Printer::maxFeedrate[E_AXIS];
        limitInterval = RMath::max(axisInterval[E_AXIS], limitInterval);
    } else axisInterval[E_AXIS] = 0;
#if DRIVE_SYSTEM == DELTA
    if(axisDistanceMM[VIRTUAL_AXIS] >= 0) {// only for deltas all speeds in all directions have same limit
        axisInterval[VIRTUAL_AXIS] = axisDistanceMM[VIRTUAL_AXIS] * toTicks / (Printer::maxFeedrate[Z_AXIS]);
        limitInterval = RMath::max(axisInterval[VIRTUAL_AXIS], limitInterval);
    }
#endif
    fullInterval = limitInterval = limitInterval > LIMIT_INTERVAL ? limitInterval : LIMIT_INTERVAL; // This is our target speed
    if(limitInterval != limitInterval0) {
        // new time at full speed = limitInterval*p->stepsRemaining [ticks]
        timeForMove = (float)limitInterval * (float)stepsRemaining; // for large z-distance this overflows with long computation
    }
    float inverseTimeS = (float)F_CPU / timeForMove;
    if(isXMove()) {
        axisInterval[X_AXIS] = timeForMove / delta[X_AXIS];
        speedX = axisDistanceMM[X_AXIS] * inverseTimeS;
        if(isXNegativeMove()) speedX = -speedX;
    } else speedX = 0;
    if(isYMove()) {
        axisInterval[Y_AXIS] = timeForMove / delta[Y_AXIS];
        speedY = axisDistanceMM[Y_AXIS] * inverseTimeS;
        if(isYNegativeMove()) speedY = -speedY;
    } else speedY = 0;
    if(isZMove()) {
        axisInterval[Z_AXIS] = timeForMove / delta[Z_AXIS];
        speedZ = axisDistanceMM[Z_AXIS] * inverseTimeS;
        if(isZNegativeMove()) speedZ = -speedZ;
    } else speedZ = 0;
    if(isEMove()) {
        axisInterval[E_AXIS] = timeForMove / delta[E_AXIS];
        speedE = axisDistanceMM[E_AXIS] * inverseTimeS;
        if(isENegativeMove()) speedE = -speedE;
    } else speedE = 0;
#if NONLINEAR_SYSTEM
    axisInterval[VIRTUAL_AXIS] = limitInterval; //timeForMove/stepsRemaining;
#endif
    fullSpeed = distance * inverseTimeS;
    //long interval = axis_interval[primary_axis]; // time for every step in ticks with full speed
    //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it.
#if RAMP_ACCELERATION
    // slowest time to accelerate from v0 to limitInterval determines used acceleration
    // t = (v_end-v_start)/a
    float slowestAxisPlateauTimeRepro = 1e15; // 1/time to reduce division Unit: 1/s
    uint32_t *accel = (isEPositiveMove() ?  Printer::maxPrintAccelerationStepsPerSquareSecond : Printer::maxTravelAccelerationStepsPerSquareSecond);
#if defined(INTERPOLATE_ACCELERATION_WITH_Z) && INTERPOLATE_ACCELERATION_WITH_Z != 0
    uint32_t newAccel[4];
    float accelFac = (100.0 + (EEPROM::accelarationFactorTop() - 100.0) * Printer::currentPosition[Z_AXIS] / Printer::zLength) * 0.01;
#if INTERPOLATE_ACCELERATION_WITH_Z == 1 || INTERPOLATE_ACCELERATION_WITH_Z == 3
    newAccel[X_AXIS] = static_cast<int32_t>(accel[X_AXIS] * accelFac);
    newAccel[Y_AXIS] = static_cast<int32_t>(accel[Y_AXIS] * accelFac);
#else
    newAccel[X_AXIS] = accel[X_AXIS];
    newAccel[Y_AXIS] = accel[Y_AXIS];
#endif
#if INTERPOLATE_ACCELERATION_WITH_Z == 2 || INTERPOLATE_ACCELERATION_WITH_Z == 3
    newAccel[Z_AXIS] = static_cast<int32_t>(accel[Z_AXIS] * accelFac);
#else
    newAccel[Z_AXIS] = accel[Z_AXIS];
#endif
    newAccel[E_AXIS] = accel[E_AXIS];
    accel = newAccel;
#endif // INTERPOLATE_ACCELERATION_WITH_Z
    for(fast8_t i = 0; i < E_AXIS_ARRAY ; i++) {
        if(isMoveOfAxis(i))
            // v = a * t => t = v/a = F_CPU/(c*a) => 1/t = c*a/F_CPU
            slowestAxisPlateauTimeRepro = RMath::min(slowestAxisPlateauTimeRepro, (float)axisInterval[i] * (float)accel[i]); //  steps/s^2 * step/tick  Ticks/s^2
    }

    // Errors for delta move are initialized in timer (except extruder)
#if !NONLINEAR_SYSTEM
    error[X_AXIS] = error[Y_AXIS] = error[Z_AXIS] = error[E_AXIS] = delta[primaryAxis] >> 1;
#endif
#if NONLINEAR_SYSTEM
    error[E_AXIS] = stepsRemaining >> 1;
#endif
    invFullSpeed = 1.0 / fullSpeed;
    accelerationPrim = slowestAxisPlateauTimeRepro / axisInterval[primaryAxis]; // a = v/t = F_CPU/(c*t): Steps/s^2
    //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated
    fAcceleration = 262144.0 * (float)accelerationPrim / F_CPU; // will overflow without float!
    accelerationDistance2 = 2.0 * distance * slowestAxisPlateauTimeRepro * fullSpeed / ((float)F_CPU); // mm^2/s^2
    startSpeed = endSpeed = minSpeed = safeSpeed(drivingAxis);
    if(startSpeed > Printer::feedrate)
        startSpeed = endSpeed = minSpeed = Printer::feedrate;
    // Can accelerate to full speed within the line
    if (startSpeed * startSpeed + accelerationDistance2 >= fullSpeed * fullSpeed)
        setNominalMove();

    vMax = F_CPU / fullInterval; // maximum steps per second, we can reach
    // if(p->vMax>46000)  // gets overflow in N computation
    //   p->vMax = 46000;
    //p->plateauN = (p->vMax*p->vMax/p->accelerationPrim)>>1;
#if USE_ADVANCE
    if(!isXYZMove() || !isEPositiveMove()) {
#if ENABLE_QUADRATIC_ADVANCE
        advanceRate = 0; // No head move or E move only or sucking filament back
        advanceFull = 0;
#endif
        advanceL = 0;
    } else {
        float advlin = fabs(speedE) * Extruder::current->advanceL * 0.001 * Printer::axisStepsPerMM[E_AXIS];
        advanceL = (uint16_t)((65536L * advlin) / vMax); //advanceLscaled = (65536*vE*k2)/vMax
#if ENABLE_QUADRATIC_ADVANCE
        advanceFull = 65536 * Extruder::current->advanceK * speedE * speedE; // Steps*65536 at full speed
        long steps = (HAL::U16SquaredToU32(vMax)) / (accelerationPrim << 1); // v^2/(2*a) = steps needed to accelerate from 0-vMax
        advanceRate = advanceFull / steps;
        if((advanceFull >> 16) > maxadv) {
            maxadv = (advanceFull >> 16);
            maxadvspeed = fabs(speedE);
        }
#endif
        if(advlin > maxadv2) {
            maxadv2 = advlin;
            maxadvspeed = fabs(speedE);
        }
    }
#endif
    UI_MEDIUM; // do check encoder
    updateTrapezoids();
    // how much steps on primary axis do we need to reach target feedrate
    //p->plateauSteps = (long) (((float)p->acceleration *0.5f / slowest_axis_plateau_time_repro + p->vMin) *1.01f/slowest_axis_plateau_time_repro);
#else
#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
    advanceRate = 0; // No advance for constant speeds
    advanceFull = 0;
#endif
#endif
#endif

#ifdef DEBUG_STEPCOUNT
// Set in delta move calculation
#if !NONLINEAR_SYSTEM
    totalStepsRemaining = delta[X_AXIS] + delta[Y_AXIS] + delta[Z_AXIS];
#endif
#endif
#ifdef DEBUG_QUEUE_MOVE
    if(Printer::debugEcho()) {
        logLine();
        Com::printFLN(Com::tDBGLimitInterval, limitInterval);
        Com::printFLN(Com::tDBGMoveDistance, distance);
        Com::printFLN(Com::tDBGCommandedFeedrate, Printer::feedrate);
        Com::printFLN(Com::tDBGConstFullSpeedMoveTime, timeForMove);
    }
#endif
    // Make result permanent
    if (pathOptimize) waitRelax = 70;
    pushLine();
    DEBUG_MEMORY;
}

/**
This is the path planner.

It goes from the last entry and tries to increase the end speed of previous moves in a fashion that the maximum jerk
is never exceeded. If a segment with reached maximum speed is met, the planner stops. Everything left from this
is already optimal from previous updates.
The first 2 entries in the queue are not checked. The first is the one that is already in print and the following will likely to become active.

The method is called before lines_count is increased!
*/
void PrintLine::updateTrapezoids() {
    ufast8_t first = linesWritePos;
    PrintLine *firstLine;
    PrintLine *act = &lines[linesWritePos];
    InterruptProtectedBlock noInts;

    // First we find out how far back we could go with optimization.

    ufast8_t maxfirst = linesPos; // first non fixed segment we might change
    if(maxfirst != linesWritePos)
        nextPlannerIndex(maxfirst); // don't touch the line printing
    // Now ignore enough segments to gain enough time for path planning
    millis_t timeleft = 0;
    // Skip as many stored moves as needed to gain enough time for computation
#if PRINTLINE_CACHE_SIZE < 10
#define minTime 4500L * PRINTLINE_CACHE_SIZE
#else
#define minTime 45000L
#endif
    while(timeleft < minTime && maxfirst != linesWritePos) {
        timeleft += lines[maxfirst].timeInTicks;
        nextPlannerIndex(maxfirst);
    }
    // Search last fixed element
    while(first != maxfirst && !lines[first].isEndSpeedFixed())
        previousPlannerIndex(first);
    if(first != linesWritePos && lines[first].isEndSpeedFixed())
        nextPlannerIndex(first);
    // now first points to last segment before the end speed is fixed
    // so start speed is also fixed.

    if(first == linesWritePos) { // Nothing to plan, only new element present
        act->block(); // Prevent stepper interrupt from using this
        noInts.unprotect();
        act->setStartSpeedFixed(true);
        act->updateStepsParameter();
        act->unblock();
        return;
    }
    // now we have at least one additional move for optimization
    // that is not a wait move
    // First is now the new element or the first element with non fixed end speed.
    // anyhow, the start speed of first is fixed
    firstLine = &lines[first];
    firstLine->block(); // don't let printer touch this or following segments during update
    noInts.unprotect();
    ufast8_t previousIndex = linesWritePos;
    previousPlannerIndex(previousIndex);
    PrintLine *previous = &lines[previousIndex]; // segment before the one we are inserting
#if DRIVE_SYSTEM != DELTA
    // filters z-move<->not z-move
    /*    if((previous->primaryAxis == Z_AXIS && act->primaryAxis != Z_AXIS) || (previous->primaryAxis != Z_AXIS && act->primaryAxis == Z_AXIS))
        {
            previous->setEndSpeedFixed(true);
            act->setStartSpeedFixed(true);
            act->updateStepsParameter();
            firstLine->unblock();
            return;
        }*/
#endif // DRIVE_SYSTEM

    if(previous->isEOnlyMove() != act->isEOnlyMove()) {
        previous->maxJunctionSpeed = previous->endSpeed; // act->startSpeed; // maybe remove this. Previous should be at minimum and systems have nothing in common
        previous->setEndSpeedFixed(true);
        act->setStartSpeedFixed(true);
        act->updateStepsParameter();
        firstLine->unblock();
        return;
    } else {
        computeMaxJunctionSpeed(previous, act); // Set maximum junction speed if we have a real move before
    }
    // Increase speed if possible neglecting current speed
    backwardPlanner(linesWritePos, first);
    // Reduce speed to reachable speeds
    forwardPlanner(first);

#ifdef DEBUG_PLANNER
    if(Printer::debugEcho()) {
        Com::printF(PSTR("Planner: "), (int)linesCount);
        previousPlannerIndex(first);
        Com::printF(PSTR(" F "), lines[first].startSpeed, 1);
        Com::printF(PSTR(" - "), lines[first].endSpeed, 1);
        Com::printF(PSTR("("), lines[first].maxJunctionSpeed, 1);
        Com::printF(PSTR(","), (int)lines[first].joinFlags);
        nextPlannerIndex(first);
    }
#endif
    // Update precomputed data
    do {
        lines[first].updateStepsParameter();
#ifdef DEBUG_PLANNER
        if(Printer::debugEcho()) {
            Com::printF(PSTR(" / "), lines[first].startSpeed, 1);
            Com::printF(PSTR(" - "), lines[first].endSpeed, 1);
            Com::printF(PSTR("("), lines[first].maxJunctionSpeed, 1);
            Com::printF(PSTR(","), (int)lines[first].joinFlags);
#ifdef DEBUG_QUEUE_MOVE
            Com::println();
#endif
        }
#endif
        //noInts.protect();
        lines[first].unblock();  // start with first block to release next used segment as early as possible
        nextPlannerIndex(first);
        lines[first].block();
        //noInts.unprotect();
    } while(first != linesWritePos);
    act->updateStepsParameter();
    act->unblock();
#ifdef DEBUG_PLANNER
    if(Printer::debugEcho()) {
        Com::printF(PSTR(" / "), lines[first].startSpeed, 1);
        Com::printF(PSTR(" - "), lines[first].endSpeed, 1);
        Com::printF(PSTR("("), lines[first].maxJunctionSpeed, 1);
        Com::printFLN(PSTR(","), (int)lines[first].joinFlags);
    }
#endif
}

/* Computes the maximum junction speed of the newly added segment under
optimal conditions. There is no guarantee that the previous move will be able to reach the
speed at all, but if it could exceed it will never exceed this theoretical limit.

if you define ALTERNATIVE_JERK the new jerk computations are used. These
use the cosine of the angle and the maximum speed
Jerk = (1-cos(alpha))*min(v1,v2)
This sets jerk to 0 on zero angle change.

        Old               New
0°:       0               0
30°:     51,8             13.4
45°:     76.53            29.3
90°:    141               100
180°:   200               200


Speed from 100 to 200
        Old               New(min)   New(max)
0°:     100               0          0
30°:    123,9             13.4       26.8
45°:    147.3             29.3       58.6
90°:    223               100        200
180°:   300               200        400

*/
inline void PrintLine::computeMaxJunctionSpeed(PrintLine *previous, PrintLine *current) {
#if NONLINEAR_SYSTEM
    /*  if (previous->moveID == current->moveID)   // Avoid computing junction speed for split nonlinear lines
      {
          if(previous->fullSpeed > current->fullSpeed)
              previous->maxJunctionSpeed = current->fullSpeed;
          else
              previous->maxJunctionSpeed = previous->fullSpeed;
          return;
      }*/
#endif
#if USE_ADVANCE
    if(Printer::isAdvanceActivated()) {
        // if we start/stop extrusion we need to do so with lowest possible end speed
        // or advance would leave a drolling extruder and can not adjust fast enough.
        if(previous->isEMove() != current->isEMove()) {
            previous->setEndSpeedFixed(true);
            current->setStartSpeedFixed(true);
            previous->endSpeed = current->startSpeed = previous->maxJunctionSpeed = RMath::min(previous->endSpeed, current->startSpeed);
            previous->invalidateParameter();
            current->invalidateParameter();
            return;
        }
    }
#endif // USE_ADVANCE
    // if we are here we have to identical move types
    // either pure extrusion -> pure extrusion or
    // move -> move (with or without extrusion)
    // First we compute the normalized jerk for speed 1
    float factor = 1.0;
    float lengthFactor = 1.0;
#ifdef REDUCE_ON_SMALL_SEGMENTS
    if(previous->distance < MAX_JERK_DISTANCE)
        lengthFactor = static_cast<float>(MAX_JERK_DISTANCE * MAX_JERK_DISTANCE) / (previous->distance * previous->distance);
#endif
    float maxJoinSpeed = RMath::min(current->fullSpeed, previous->fullSpeed);
#if (DRIVE_SYSTEM == DELTA) // No point computing Z Jerk separately for delta moves
#ifdef ALTERNATIVE_JERK
    float jerk = maxJoinSpeed * lengthFactor * (1.0 - (current->speedX * previous->speedX + current->speedY * previous->speedY + current->speedZ * previous->speedZ) / (current->fullSpeed * previous->fullSpeed));
#else
    float dx = current->speedX - previous->speedX;
    float dy = current->speedY - previous->speedY;
    float dz = current->speedZ - previous->speedZ;
    float jerk = sqrt(dx * dx + dy * dy + dz * dz) * lengthFactor;
#endif // ALTERNATIVE_JERK
#else // DELTA
#ifdef ALTERNATIVE_JERK
    float jerk = maxJoinSpeed * lengthFactor * (1.0 - (current->speedX * previous->speedX + current->speedY * previous->speedY + current->speedZ * previous->speedZ) / (current->fullSpeed * previous->fullSpeed));
#else
    float dx = current->speedX - previous->speedX;
    float dy = current->speedY - previous->speedY;
    float jerk = sqrt(dx * dx + dy * dy) * lengthFactor;
#endif // ALTERNATIVE_JERK
#endif // DELTA
    if(jerk > Printer::maxJerk) {
        factor = Printer::maxJerk / jerk; // always < 1.0!
        if(factor * maxJoinSpeed * 2.0 < Printer::maxJerk)
            factor = Printer::maxJerk / (2.0 * maxJoinSpeed);
    }
#if DRIVE_SYSTEM != DELTA
    if((previous->dir | current->dir) & ZSTEP) {
        float dz = fabs(current->speedZ - previous->speedZ);
        if(dz > Printer::maxZJerk)
            factor = RMath::min(factor, Printer::maxZJerk / dz);
    }
#endif
    float eJerk = fabs(current->speedE - previous->speedE);
    if(eJerk > Extruder::current->maxStartFeedrate) {
        factor = RMath::min(factor, Extruder::current->maxStartFeedrate / eJerk);
	}
    previous->maxJunctionSpeed = maxJoinSpeed * factor; // set speed limit
#ifdef DEBUG_QUEUE_MOVE
    if(Printer::debugEcho()) {
        Com::printF(PSTR("ID:"), (int)previous);
        Com::printFLN(PSTR(" MJ:"), previous->maxJunctionSpeed);
    }
#endif // DEBUG_QUEUE_MOVE
}

/** Update parameter used by updateTrapezoids

Computes the acceleration/deceleration steps and advanced parameter associated.
*/
void PrintLine::updateStepsParameter() {
    if(areParameterUpToDate() || isWarmUp()) return;
    float startFactor = startSpeed * invFullSpeed;
    float endFactor   = endSpeed   * invFullSpeed;
    vStart = vMax * startFactor; //starting speed
    vEnd   = vMax * endFactor;

#if CPU_ARCH == ARCH_AVR
    uint32_t vmax2 = HAL::U16SquaredToU32(vMax);
    accelSteps = ((vmax2 - HAL::U16SquaredToU32(vStart)) / (accelerationPrim << 1)) + 1; // Always add 1 for missing precision
    decelSteps = ((vmax2 - HAL::U16SquaredToU32(vEnd))  / (accelerationPrim << 1)) + 1;
#else
    uint64_t vmax2 = static_cast<uint64_t>(vMax) * static_cast<uint64_t>(vMax);
    accelSteps = ((vmax2 - static_cast<uint64_t>(vStart) * static_cast<uint64_t>(vStart)) / (accelerationPrim << 1)) + 1; // Always add 1 for missing precision
    decelSteps = ((vmax2 - static_cast<uint64_t>(vEnd) * static_cast<uint64_t>(vEnd)) / (accelerationPrim << 1)) + 1;
#endif

#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
    advanceStart = (float)advanceFull * startFactor * startFactor;
    advanceEnd   = (float)advanceFull * endFactor   * endFactor;
#endif
#endif
    if(static_cast<int32_t>(accelSteps + decelSteps) >= stepsRemaining) { // can't reach limit speed
        uint32_t red = (accelSteps + decelSteps - stepsRemaining) >> 1;
        accelSteps = accelSteps - RMath::min(static_cast<int32_t>(accelSteps), static_cast<int32_t>(red));
        decelSteps = decelSteps - RMath::min(static_cast<int32_t>(decelSteps), static_cast<int32_t>(red));
    }
    setParameterUpToDate();
#ifdef DEBUG_QUEUE_MOVE
    if(Printer::debugEcho()) {
        Com::printFLN(Com::tDBGId, (int)this);
        Com::printF(Com::tDBGVStartEnd, (long)vStart);
        Com::printFLN(Com::tSlash, (long)vEnd);
        Com::printF(Com::tDBAccelSteps, (long)accelSteps);
        Com::printF(Com::tSlash, (long)decelSteps);
        Com::printFLN(Com::tSlash, (long)stepsRemaining);
        Com::printF(Com::tDBGStartEndSpeed, startSpeed, 1);
        Com::printFLN(Com::tSlash, endSpeed, 1);
        Com::printFLN(Com::tDBGFlags, (uint32_t)flags);
        Com::printFLN(Com::tDBGJoinFlags, (uint32_t)joinFlags);
    }
#endif
}

/**
Compute the maximum speed from the last entered move.
The backwards planner traverses the moves from last to first looking at deceleration. The RHS of the accelerate/decelerate ramp.

start = last line inserted
last = last element until we check
*/
inline void PrintLine::backwardPlanner(ufast8_t start, ufast8_t last) {
    PrintLine *act = &lines[start], *previous;
    float lastJunctionSpeed = act->endSpeed; // Start always with safe speed

    //PREVIOUS_PLANNER_INDEX(last); // Last element is already fixed in start speed
    while(start != last) {
        previousPlannerIndex(start);
        previous = &lines[start];
        previous->block();
        // Avoid speed calculation once cruising in split delta move
#if NONLINEAR_SYSTEM
        /*if (previous->moveID == act->moveID && lastJunctionSpeed == previous->maxJunctionSpeed)
        {
            act->startSpeed = RMath::max(act->minSpeed, previous->endSpeed = lastJunctionSpeed);
            previous->invalidateParameter();
            act->invalidateParameter();
        }*/
#endif

        /* if(prev->isEndSpeedFixed())   // Nothing to update from here on, happens when path optimize disabled
         {
             act->setStartSpeedFixed(true);
             return;
         }*/

        // Avoid speed calculations if we know we can accelerate within the line
        lastJunctionSpeed = (act->isNominalMove() ? act->fullSpeed : sqrt(lastJunctionSpeed * lastJunctionSpeed + act->accelerationDistance2)); // acceleration is acceleration*distance*2! What can be reached if we try?
        // If that speed is more that the maximum junction speed allowed then ...
        if(lastJunctionSpeed >= previous->maxJunctionSpeed) { // Limit is reached
            // If the previous line's end speed has not been updated to maximum speed then do it now
            if(previous->endSpeed != previous->maxJunctionSpeed) {
                previous->invalidateParameter(); // Needs recomputation
                previous->endSpeed = RMath::max(previous->minSpeed, previous->maxJunctionSpeed); // possibly unneeded???
            }
            // If actual line start speed has not been updated to maximum speed then do it now
            if(act->startSpeed != previous->maxJunctionSpeed) {
                act->startSpeed = RMath::max(act->minSpeed, previous->maxJunctionSpeed); // possibly unneeded???
                act->invalidateParameter();
            }
            lastJunctionSpeed = previous->endSpeed;
        } else {
            // Block previous end and act start as calculated speed and recalculate plateau speeds (which could move the speed higher again)
            act->startSpeed = RMath::max(act->minSpeed, lastJunctionSpeed);
            lastJunctionSpeed = previous->endSpeed = RMath::max(lastJunctionSpeed, previous->minSpeed);
            previous->invalidateParameter();
            act->invalidateParameter();
        }
        act = previous;
    } // while loop
}

void PrintLine::forwardPlanner(ufast8_t first) {
    PrintLine *act;
    PrintLine *next = &lines[first];
    float vmaxRight;
    float leftSpeed = next->startSpeed;
    while(first != linesWritePos) { // All except last segment, which has fixed end speed
        act = next;
        nextPlannerIndex(first);
        next = &lines[first];
        /* if(act->isEndSpeedFixed())
         {
             leftSpeed = act->endSpeed;
             continue; // Nothing to do here
         }*/
        // Avoid speed calculate once cruising in split delta move
#if NONLINEAR_SYSTEM
        /*        if (act->moveID == next->moveID && act->endSpeed == act->maxJunctionSpeed)
                {
                    act->startSpeed = leftSpeed;
                    leftSpeed       = act->endSpeed;
                    act->setEndSpeedFixed(true);
                    next->setStartSpeedFixed(true);
                    continue;
                }*/
#endif
        // Avoid speed calculates if we know we can accelerate within the line.
        vmaxRight = (act->isNominalMove() ? act->fullSpeed : sqrt(leftSpeed * leftSpeed + act->accelerationDistance2));
        if(vmaxRight > act->endSpeed) { // Could be higher next run?
            if(leftSpeed < act->minSpeed) {
                leftSpeed = act->minSpeed;
                act->endSpeed = sqrt(leftSpeed * leftSpeed + act->accelerationDistance2);
            }
            act->startSpeed = leftSpeed;
            next->startSpeed = leftSpeed = RMath::max(RMath::min(act->endSpeed, act->maxJunctionSpeed), next->minSpeed);
            if(act->endSpeed == act->maxJunctionSpeed) { // Full speed reached, don't compute again!
                act->setEndSpeedFixed(true);
                next->setStartSpeedFixed(true);
            }
            act->invalidateParameter();
        } else { // We can accelerate full speed without reaching limit, which is as fast as possible. Fix it!
            act->fixStartAndEndSpeed();
            act->invalidateParameter();
            if(act->minSpeed > leftSpeed) {
                leftSpeed = act->minSpeed;
                vmaxRight = sqrt(leftSpeed * leftSpeed + act->accelerationDistance2);
            }
            act->startSpeed = leftSpeed;
            act->endSpeed = RMath::max(act->minSpeed, vmaxRight);
            next->startSpeed = leftSpeed = RMath::max(RMath::min(act->endSpeed, act->maxJunctionSpeed), next->minSpeed);
            next->setStartSpeedFixed(true);
        }
    } // While
    next->startSpeed = RMath::max(next->minSpeed, leftSpeed); // This is the new segment, which is updated anyway, no extra flag needed.
}


inline float PrintLine::safeSpeed(fast8_t drivingAxis) {
	float xyMin = Printer::maxJerk * 0.5;
	float mz = 0;
    float safe(xyMin);
#if DRIVE_SYSTEM != DELTA
    if(isZMove()) {
        mz = Printer::maxZJerk * 0.5;
        if(isXOrYMove()) {
            if(fabs(speedZ) > mz)
                safe = RMath::min(safe, mz * fullSpeed / fabs(speedZ));
        } else {
            safe = mz;
        }
    }
#endif
    if(isEMove()) {
        if(isXYZMove())
            safe = RMath::min(safe, 0.5 * Extruder::current->maxStartFeedrate * fullSpeed / fabs(speedE));
        else
            safe = 0.5 * Extruder::current->maxStartFeedrate; // This is a retraction move
    }
    // Check for minimum speeds needed for numerical robustness
#if DRIVE_SYSTEM == DELTA
    if(drivingAxis == X_AXIS || drivingAxis == Y_AXIS || drivingAxis == Z_AXIS) // enforce minimum speed for numerical stability of explicit speed integration
    safe = RMath::max(xyMin, safe);
#else
    if(drivingAxis == X_AXIS || drivingAxis == Y_AXIS) // enforce minimum speed for numerical stability of explicit speed integration
        safe = RMath::max(xyMin, safe);
    else if(drivingAxis == Z_AXIS)
    {
        safe = RMath::max(mz, safe);
    }
#endif
    return RMath::min(safe, fullSpeed);
}


/** Check if move is new. If it is insert some dummy moves to allow the path optimizer to work since it does
not act on the first two moves in the queue. The stepper timer will spot these moves and leave some time for
processing.
*/
uint8_t PrintLine::insertWaitMovesIfNeeded(uint8_t pathOptimize, uint8_t waitExtraLines) {
    if(linesCount == 0 && waitRelax == 0 && pathOptimize) { // First line after some time - warm up needed
        //return 0;
#if NONLINEAR_SYSTEM
        uint8_t w = 3;
#else
        uint8_t w = 4;
#endif
        while(w--) {
            PrintLine *p = getNextWriteLine();
            p->flags = FLAG_WARMUP;
            p->joinFlags = FLAG_JOIN_STEPPARAMS_COMPUTED | FLAG_JOIN_END_FIXED | FLAG_JOIN_START_FIXED;
            p->dir = 0;
            p->setWaitForXLinesFilled(w + waitExtraLines);
#if NONLINEAR_SYSTEM
            p->setWaitTicks(300000);
            p->moveID = lastMoveID++;
#else
            p->setWaitTicks(100000);
#endif // NONLINEAR_SYSTEM
            pushLine();
        }
        //Com::printFLN(PSTR("InsertWait"));
        return 1;
    }
    return 0;
}

void PrintLine::LaserWarmUp( uint32_t wait) {
    PrintLine *p = getNextWriteLine();
    p->flags = FLAG_WARMUP;
    p->joinFlags = FLAG_JOIN_STEPPARAMS_COMPUTED | FLAG_JOIN_END_FIXED | FLAG_JOIN_START_FIXED;
    p->dir = 1;
    p->setWaitForXLinesFilled(1);
    p->setWaitTicks(long(wait * (F_CPU / 1000))); //in ms
    pushLine();
    Com::printFLN(PSTR("Laser Warmup"));
}

void PrintLine::logLine() {
#ifdef DEBUG_QUEUE_MOVE
    Com::printFLN(Com::tDBGId, (int)this);
    Com::printArrayFLN(Com::tDBGDelta, delta);
    Com::printFLN(Com::tDBGDir, (uint32_t)dir);
    Com::printFLN(Com::tDBGFlags, (uint32_t)flags);
    Com::printFLN(Com::tDBGFullSpeed, fullSpeed);
    Com::printFLN(Com::tDBGVMax, (int32_t)vMax);
    Com::printFLN(Com::tDBGAcceleration, accelerationDistance2);
    Com::printFLN(Com::tDBGAccelerationPrim, (int32_t)accelerationPrim);
    Com::printFLN(Com::tDBGRemainingSteps, stepsRemaining);
#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
    Com::printFLN(Com::tDBGAdvanceFull, advanceFull >> 16);
    Com::printFLN(Com::tDBGAdvanceRate, advanceRate);
#endif
#endif
#endif // DEBUG_QUEUE_MOVE
}

void PrintLine::waitForXFreeLines(uint8_t b, bool allowMoves) {
    while(getLinesCount() + b > PRINTLINE_CACHE_SIZE) { // wait for a free entry in movement cache
        //GCode::readFromSerial();
        Commands::checkForPeriodicalActions(allowMoves);
    }
}

#ifdef FAST_COREXYZ
uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t corePosSteps[]) {
#if DRIVE_SYSTEM == XY_GANTRY
    //1 = z axis + xy H-gantry (x_motor = x+y, y_motor = x-y)
    corePosSteps[A_TOWER] = cartesianPosSteps[X_AXIS] + cartesianPosSteps[Y_AXIS];
    corePosSteps[B_TOWER] = cartesianPosSteps[X_AXIS] - cartesianPosSteps[Y_AXIS];
    corePosSteps[C_TOWER] = cartesianPosSteps[Z_AXIS];
#elif DRIVE_SYSTEM == YX_GANTRY
    // 2 = z axis + xy H-gantry (x_motor = x+y, y_motor = y-x)
    corePosSteps[A_TOWER] = cartesianPosSteps[X_AXIS] + cartesianPosSteps[Y_AXIS];
    corePosSteps[B_TOWER] = cartesianPosSteps[Y_AXIS] - cartesianPosSteps[X_AXIS];
    corePosSteps[C_TOWER] = cartesianPosSteps[Z_AXIS];
#elif DRIVE_SYSTEM == XZ_GANTRY
    // 8 = y axis + xz H-gantry (x_motor = x+z, z_motor = x-z)
    corePosSteps[A_TOWER] = cartesianPosSteps[X_AXIS] + cartesianPosSteps[Z_AXIS];
    corePosSteps[C_TOWER] = cartesianPosSteps[X_AXIS] - cartesianPosSteps[Z_AXIS];
    corePosSteps[B_TOWER] = cartesianPosSteps[Y_AXIS];
#elif DRIVE_SYSTEM == ZX_GANTRY
    //9 = y axis + xz H-gantry (x_motor = x+z, z_motor = z-x)
    corePosSteps[A_TOWER] = cartesianPosSteps[X_AXIS] + cartesianPosSteps[Z_AXIS];
    corePosSteps[C_TOWER] = cartesianPosSteps[Z_AXIS] - cartesianPosSteps[X_AXIS];
    corePosSteps[B_TOWER] = cartesianPosSteps[Y_AXIS];
#elif DRIVE_SYSTEM == GANTRY_FAKE
    corePosSteps[A_TOWER] = cartesianPosSteps[X_AXIS];
    corePosSteps[B_TOWER] = cartesianPosSteps[Y_AXIS];
    corePosSteps[C_TOWER] = cartesianPosSteps[Z_AXIS];
#endif
    return 1;
}
#endif

#if DRIVE_SYSTEM == DELTA
// pick one for verbose the other silent
#define RETURN_0(s) { Com::printErrorFLN(PSTR(s)); return 0; }
/*#define RETURN_0(s) { Com::print(s " "); SHOWS(temp); SHOWS(opt);\
   SHOWS(cartesianPosSteps[Z_AXIS]);\
   SHOWS(towerAMinSteps); ;\
   SHOWS(deltaPosSteps[A_TOWER]); \
   SHOWS(Printer::deltaAPosYSteps);\
   SHOWS(cartesianPosSteps[Y_AXIS]); \
   SHOW(Printer::deltaDiagonalStepsSquaredA.l);  return 0; }
   */
/**
  Calculate the delta tower position from a Cartesian position
  @param cartesianPosSteps Array containing Cartesian coordinates.
  @param deltaPosSteps Result array with tower coordinates.
  @returns 1 if Cartesian coordinates have a valid delta tower position 0 if not.
*/
uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t deltaPosSteps[]) {
    int32_t zSteps = cartesianPosSteps[Z_AXIS];
#if DISTORTION_CORRECTION
    static int cnt = 0;
    static int32_t lastZSteps = 9999999;
    static int32_t lastZCorrection = 0;
    cnt++;
    if(cnt >= DISTORTION_UPDATE_FREQUENCY || lastZSteps != zSteps) {
        cnt = 0;
        lastZSteps = zSteps;
        lastZCorrection = Printer::distortion.correct(cartesianPosSteps[X_AXIS], cartesianPosSteps[Y_AXIS], cartesianPosSteps[Z_AXIS]);
    }
    zSteps += lastZCorrection;
#endif
    if(Printer::isLargeMachine()) {
#ifdef SUPPORT_64_BIT_MATH
        // 64 bit is better for precision, so we use that if available.
        // A TOWER height
        uint64_t temp = RMath::absLong(Printer::deltaAPosYSteps - cartesianPosSteps[Y_AXIS]);
        uint64_t opt = Printer::deltaDiagonalStepsSquaredA.L;

        temp *= temp;
        if (opt < temp)
            RETURN_0("Apos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaAPosXSteps - cartesianPosSteps[X_AXIS]);
        temp *= temp;
        if (opt < temp)
            RETURN_0("Apos x square ");

        deltaPosSteps[A_TOWER] = HAL::integer64Sqrt(opt - temp) + zSteps;
        if (deltaPosSteps[A_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("A hit floor");

        // B TOWER height
        temp = RMath::absLong(Printer::deltaBPosYSteps - cartesianPosSteps[Y_AXIS]);
        opt = Printer::deltaDiagonalStepsSquaredB.L;
        temp *= temp;
        if (opt < temp)
            RETURN_0("Bpos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaBPosXSteps - cartesianPosSteps[X_AXIS]);
        temp *= temp;
        if (opt < temp)
            RETURN_0("Bpos x square ");

        deltaPosSteps[B_TOWER] = HAL::integer64Sqrt(opt - temp) + zSteps ;
        if (deltaPosSteps[B_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("B hit floor");

        // C TOWER height
        temp = RMath::absLong(Printer::deltaCPosYSteps - cartesianPosSteps[Y_AXIS]);
        opt = Printer::deltaDiagonalStepsSquaredC.L ;

        temp = temp * temp;
        if ( opt < temp )
            RETURN_0("Cpos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaCPosXSteps - cartesianPosSteps[X_AXIS]);
        temp = temp * temp;
        if ( opt < temp )
            RETURN_0("Cpos x square ");

        deltaPosSteps[C_TOWER] = HAL::integer64Sqrt(opt - temp) + zSteps;
        if (deltaPosSteps[C_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("C hit floor");
#else
        float temp = Printer::deltaAPosYSteps - cartesianPosSteps[Y_AXIS];
        float opt = Printer::deltaDiagonalStepsSquaredA.f - temp * temp;
        float temp2 = Printer::deltaAPosXSteps - cartesianPosSteps[X_AXIS];
        if ((temp = opt - temp2 * temp2) >= 0)
            deltaPosSteps[A_TOWER] = floor(0.5 + sqrt(temp)
                                           + zSteps);
        else
            return 0;
        if (deltaPosSteps[A_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive()) return 0;

        temp = Printer::deltaBPosYSteps - cartesianPosSteps[Y_AXIS];
        opt = Printer::deltaDiagonalStepsSquaredB.f - temp * temp;
        temp2 = Printer::deltaBPosXSteps - cartesianPosSteps[X_AXIS];
        if ((temp = opt - temp2 * temp2) >= 0)
            deltaPosSteps[B_TOWER] = floor(0.5 + sqrt(temp)
                                           + zSteps);
        else
            return 0;
        if (deltaPosSteps[B_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive()) return 0;

        temp = Printer::deltaCPosYSteps - cartesianPosSteps[Y_AXIS];
        opt = Printer::deltaDiagonalStepsSquaredC.f - temp * temp;
        temp2 = Printer::deltaCPosXSteps - cartesianPosSteps[X_AXIS];
        if ((temp = opt - temp2 * temp2) >= 0)
            deltaPosSteps[C_TOWER] = floor(0.5 + sqrt(temp)
                                           + zSteps);
        else
            return 0;
        if (deltaPosSteps[C_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive()) return 0;

        return 1;
#endif
    } else {
        // As we are right on the edge of many printers arm lengths, this is rewritten to use unsigned long
        // This allows 52% longer arms to be used without performance penalty
        // the code is a bit longer, because we cannot use negative to test for invalid conditions
        // Also, previous code did not check for overflow of squared result
        // Overflow is also detected as a fault condition

        const uint32_t LIMIT = 65534; // Largest squarable int without overflow;

        // A TOWER height
        uint32_t temp = RMath::absLong(Printer::deltaAPosYSteps - cartesianPosSteps[Y_AXIS]);
        uint32_t opt = Printer::deltaDiagonalStepsSquaredA.l;

        if (temp > LIMIT)
            RETURN_0("Apos y steps ");
        temp *= temp;
        if (opt < temp)
            RETURN_0("Apos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaAPosXSteps - cartesianPosSteps[X_AXIS]);
        if (temp > LIMIT)
            RETURN_0("Apos x steps ");
        temp *= temp;
        if (opt < temp)
            RETURN_0("Apos x square ");

        deltaPosSteps[A_TOWER] = SQRT(opt - temp) + zSteps;
        if (deltaPosSteps[A_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("A hit floor");

        // B TOWER height
        temp = RMath::absLong(Printer::deltaBPosYSteps - cartesianPosSteps[Y_AXIS]);
        opt = Printer::deltaDiagonalStepsSquaredB.l;

        if (temp > LIMIT)
            RETURN_0("Bpos y steps ");
        temp *= temp;
        if (opt < temp)
            RETURN_0("Bpos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaBPosXSteps - cartesianPosSteps[X_AXIS]);
        if (temp > LIMIT )
            RETURN_0("Bpos x steps ");
        temp *= temp;
        if (opt < temp)
            RETURN_0("Bpos x square ");

        deltaPosSteps[B_TOWER] = SQRT(opt - temp) + zSteps ;
        if (deltaPosSteps[B_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("B hit floor");

        // C TOWER height
        temp = RMath::absLong(Printer::deltaCPosYSteps - cartesianPosSteps[Y_AXIS]);
        opt = Printer::deltaDiagonalStepsSquaredC.l ;

        if (temp > LIMIT)
            RETURN_0("Cpos y steps ");
        temp = temp * temp;
        if ( opt < temp )
            RETURN_0("Cpos y square ");
        opt -= temp;

        temp = RMath::absLong(Printer::deltaCPosXSteps - cartesianPosSteps[X_AXIS]);
        if (temp > LIMIT)
            RETURN_0("Cpos x steps ");
        temp = temp * temp;
        if ( opt < temp )
            RETURN_0("Cpos x square ");

        deltaPosSteps[C_TOWER] = SQRT(opt - temp) + zSteps;
        if (deltaPosSteps[C_TOWER] < Printer::deltaFloorSafetyMarginSteps && !Printer::isZProbingActive())
            RETURN_0("C hit floor");
        /*
                long temp = Printer::deltaAPosYSteps - cartesianPosSteps[Y_AXIS];
                long opt = Printer::deltaDiagonalStepsSquaredA.l - temp * temp;
                long temp2 = Printer::deltaAPosXSteps - cartesianPosSteps[X_AXIS];
                if ((temp = opt - temp2 * temp2) >= 0)
        #ifdef FAST_INTEGER_SQRT
                    deltaPosSteps[A_TOWER] = HAL::integerSqrt(temp) + cartesianPosSteps[Z_AXIS];
        #else
                    deltaPosSteps[A_TOWER] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
        #endif
                else
                    return 0;

                temp = Printer::deltaBPosYSteps - cartesianPosSteps[Y_AXIS];
                opt = Printer::deltaDiagonalStepsSquaredB.l - temp * temp;
                temp2 = Printer::deltaBPosXSteps - cartesianPosSteps[X_AXIS];
                if ((temp = opt - temp2*temp2) >= 0)
        #ifdef FAST_INTEGER_SQRT
                    deltaPosSteps[B_TOWER] = HAL::integerSqrt(temp) + cartesianPosSteps[Z_AXIS];
        #else
                    deltaPosSteps[B_TOWER] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
        #endif
                else
                    return 0;

                temp = Printer::deltaCPosYSteps - cartesianPosSteps[Y_AXIS];
                opt = Printer::deltaDiagonalStepsSquaredC.l - temp * temp;
                temp2 = Printer::deltaCPosXSteps - cartesianPosSteps[X_AXIS];
                if ((temp = opt - temp2*temp2) >= 0)
        #ifdef FAST_INTEGER_SQRT
                    deltaPosSteps[C_TOWER] = HAL::integerSqrt(temp) + cartesianPosSteps[Z_AXIS];
        #else
                    deltaPosSteps[C_TOWER] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
        #endif
                else
                    return 0;*/
    }
    return 1;
}
#endif

#if DRIVE_SYSTEM==TUGA

/**
  Calculate the delta tower position from a Cartesian position
  @param cartesianPosSteps Array containing Cartesian coordinates.
  @param deltaPosSteps Result array with tower coordinates.
  @returns 1 if Cartesian coordinates have a valid delta tower position 0 if not.

  X         Y
  *        *
   \      /
    \    /
     \  /
      \/
      /
     /
    /
   /
  *  Extruder


*/
uint8_t transformCartesianStepsToDeltaSteps(int32_t cartesianPosSteps[], int32_t tugaPosSteps[]) {
    tugaPosSteps[0] = cartesianPosSteps[0];
    tugaPosSteps[2] = cartesianPosSteps[2];
    int32_t y2 = Printer::deltaBPosXSteps - cartesianPosSteps[1];
    if(Printer::isLargeMachine()) {
        float y2f = (float)y2 * (float)y2;
        float temp = Printer::deltaDiagonalStepsSquaredF - y2f;
        if(temp < 0) return 0;
        tugaPosSteps[1] = tugaPosSteps[0] + sqrt(temp);
    } else {
        y2 = y2 * y2;
        int32_t temp = Printer::deltaDiagonalStepsSquared - y2;
        if(temp < 0) return 0;
        tugaPosSteps[1] = tugaPosSteps[0] + HAL::integerSqrt(temp);
    }
    return 1;
}
#endif


#if NONLINEAR_SYSTEM

bool NonlinearSegment::checkEndstops(PrintLine *cur, bool checkall) {
    fast8_t r = 0;
    if(Printer::isZProbingActive()) {
        Endstops::update();
#if FEATURE_Z_PROBE
        if(isZNegativeMove() && Endstops::zProbe()) {
#if DRIVE_SYSTEM == DELTA
            cur->setXMoveFinished();
            cur->setYMoveFinished();
#endif
            cur->setZMoveFinished();
            //dir = 0;
            Printer::stepsRemainingAtZHit = cur->stepsRemaining;
            cur->stepsRemaining = 0;
            return true;
        }
#endif
#if DRIVE_SYSTEM == DELTA
        if(isZPositiveMove() && isXPositiveMove() && isYPositiveMove() && Endstops::anyXYZMax())
#else
        if(isZPositiveMove() && Endstops::zMax())
#endif
        {
#if DRIVE_SYSTEM == DELTA
            cur->setXMoveFinished();
            cur->setYMoveFinished();
#endif
            cur->setZMoveFinished();
            //dir = 0;
            Printer::stepsRemainingAtZHit = cur->stepsRemaining;
            return true;
        }
    } else if(checkall) {
        Endstops::update(); // do not test twice
        if(!Endstops::anyXYZ()) // very quick check for the normal case
            return false;
    }
    if(checkall) {
#if GANTRY
        // Test axis endstops based on global direction
        if(cur->isXPositiveMove() && Endstops::xMax()) {
            setXMoveFinished();
            cur->setXMoveFinished();
            r = 1;
        }
        if(cur->isYPositiveMove() && Endstops::yMax()) {
            setYMoveFinished();
            cur->setYMoveFinished();
            r = 1;
        }
        if(cur->isXNegativeMove() && Endstops::xMin()) {
            setXMoveFinished();
            cur->setXMoveFinished();
            r = 1;
        }
        if(cur->isYNegativeMove() && Endstops::yMin()) {
            setYMoveFinished();
            cur->setYMoveFinished();
            r = 1;
        }
#if MULTI_ZENDSTOP_HOMING
        {
            if(Printer::isHoming()) {
                if(isZNegativeMove()) {
                    if(Endstops::zMin())
                        Printer::multiZHomeFlags &= ~1;
                    if(Endstops::z2MinMax())
                        Printer::multiZHomeFlags &= ~2;
                    if(Printer::multiZHomeFlags == 0)
                        setZMoveFinished();
                } else if(isZPositiveMove()) {
                    if(Endstops::zMax())
                        Printer::multiZHomeFlags &= ~1;
                    if(Endstops::z2MinMax())
                        Printer::multiZHomeFlags &= ~2;
                    if(Printer::multiZHomeFlags == 0) {
                        setZMoveFinished();
                        cur->setZMoveFinished();
                        r = 1;
                    }
                }
            } else {
#if !(Z_MIN_PIN == Z_PROBE_PIN && FEATURE_Z_PROBE)
                if(isZNegativeMove() && Endstops::zMin()) {
                    setZMoveFinished();
                    cur->setZMoveFinished();
                    r = 1;
                } else 
#endif
				if(isZPositiveMove() && Endstops::zMax()) {
                    setZMoveFinished();
                    cur->setZMoveFinished();
                    r = 1;
                }
            }
        }
#else
        if(cur->isZPositiveMove() && Endstops::zMax()) {
            setZMoveFinished();
            cur->setZMoveFinished();
            r = 1;
        }
        if(cur->isZNegativeMove() && Endstops::zMin()
#if Z_MIN_PIN == Z_PROBE_PIN && FEATURE_Z_PROBE
			&& Printer::isHoming()
#endif		
		) {
            setZMoveFinished();
            cur->setZMoveFinished();
            r = 1;
        }
#endif
#else
        // endstops are per motor and do not depend on global axis movement
        if(isXPositiveMove() && Endstops::xMax()) {
#if DRIVE_SYSTEM == DELTA
            if(Printer::stepsRemainingAtXHit < 0)
                Printer::stepsRemainingAtXHit = cur->stepsRemaining;
#endif
            setXMoveFinished();
            cur->setXMoveFinished();
            r++;
        }
        if(isYPositiveMove() && Endstops::yMax()) {
#if DRIVE_SYSTEM == DELTA
            if(Printer::stepsRemainingAtYHit < 0)
                Printer::stepsRemainingAtYHit = cur->stepsRemaining;
#endif
            setYMoveFinished();
            cur->setYMoveFinished();
            r++;
        }
#if DRIVE_SYSTEM != DELTA
        if(isXNegativeMove() && Endstops::xMin()) {
            setXMoveFinished();
            cur->setXMoveFinished();
            r++;
        }
        if(isYNegativeMove() && Endstops::yMin()) {
            setYMoveFinished();
            cur->setYMoveFinished();
            r++;
        }
#endif
        if(isZPositiveMove() && Endstops::zMax()) {
#if MAX_HARDWARE_ENDSTOP_Z
            if(Printer::stepsRemainingAtZHit)
                Printer::stepsRemainingAtZHit = cur->stepsRemaining;
#endif
            setZMoveFinished();
            cur->setZMoveFinished();
            r++;
        }
        if(isZNegativeMove() && Endstops::zMin()) {
            setZMoveFinished();
            cur->setZMoveFinished();
            r++;
        }
#if DRIVE_SYSTEM == DELTA
        if(Printer::isHoming())
            return r == 3;
#endif
#endif // Not gantry        
    }
    return r != 0;
}

void PrintLine::calculateDirectionAndDelta(int32_t difference[], ufast8_t *dir, int32_t delta[]) {
    *dir = 0;
    //Find direction
    if(difference[X_AXIS] != 0) {
        if(difference[X_AXIS] < 0) {
            delta[X_AXIS] = -difference[X_AXIS];
            *dir |= XSTEP;
        } else {
            delta[X_AXIS] = difference[X_AXIS];
            *dir |= X_DIRPOS + XSTEP;
        }
    } else {
        delta[X_AXIS] = 0;
    }

    if(difference[Y_AXIS] != 0) {
        if(difference[Y_AXIS] < 0) {
            delta[Y_AXIS] = -difference[Y_AXIS];
            *dir |= YSTEP;
        } else {
            delta[Y_AXIS] = difference[Y_AXIS];
            *dir |= Y_DIRPOS + YSTEP;
        }
    } else {
        delta[Y_AXIS] = 0;
    }
    if(difference[Z_AXIS] != 0) {
        if(difference[Z_AXIS] < 0) {
            delta[Z_AXIS] = -difference[Z_AXIS];
            *dir |= ZSTEP;
        } else {
            delta[Z_AXIS] = difference[Z_AXIS];
            *dir |= Z_DIRPOS + ZSTEP;
        }
    } else {
        delta[Z_AXIS] = 0;
    }
    if(difference[E_AXIS] != 0) {
        if(difference[E_AXIS] < 0) {
            delta[E_AXIS] = -difference[E_AXIS];
            *dir |= ESTEP;
        } else {
            delta[E_AXIS] = difference[E_AXIS];
            *dir |= E_DIRPOS + ESTEP;
        }
    } else {
        delta[E_AXIS] = 0;
    }
}
/**
  Calculate and cache the delta robot positions of the Cartesian move in a line.
  @return The largest delta axis move in a single segment
  @param p The line to examine.
*/
inline uint16_t PrintLine::calculateNonlinearSubSegments(uint8_t softEndstop) {
    fast8_t i;
    int32_t delta, diff;
    int32_t destinationSteps[Z_AXIS_ARRAY], destinationDeltaSteps[TOWER_ARRAY];
    // Save current position
#if (CPU_ARCH == ARCH_AVR) && !EXACT_DELTA_MOVES
    for(uint8_t i = 0; i < Z_AXIS_ARRAY; i++)
        destinationSteps[i] = Printer::currentPositionSteps[i];
#else
    float dx[Z_AXIS_ARRAY];
    for(int i = 0; i < Z_AXIS_ARRAY; i++)
        dx[i] = static_cast<float>(Printer::destinationSteps[i] - Printer::currentPositionSteps[i]) / static_cast<float>(numNonlinearSegments);
#endif
//  out.println_byte_P(PSTR("Calculate delta segments:"), p->numDeltaSegments);
#ifdef DEBUG_STEPCOUNT
    totalStepsRemaining = 0;
#endif

    uint16_t maxAxisSteps = 0;
    for (int s = numNonlinearSegments; s > 0; s--) {
        NonlinearSegment *d = &segments[s - 1];

#if (CPU_ARCH == ARCH_AVR) && !EXACT_DELTA_MOVES
        for(i = 0; i < Z_AXIS_ARRAY; i++) {
            // End of segment in Cartesian steps

            // This method generates small waves which get larger with increasing number of delta segments. smaller?
            diff = Printer::destinationSteps[i] - destinationSteps[i];
            if(s == 1)
                destinationSteps[i] += diff;
            else if(s == 2)
                destinationSteps[i] += (diff >> 1);
            else if(s == 4)
                destinationSteps[i] += (diff >> 2);
            else if(diff < 0)
                destinationSteps[i] -= HAL::Div4U2U(-diff, s);
            else
                destinationSteps[i] += HAL::Div4U2U(diff, s);
        }
#else
        float segment = static_cast<float>(numNonlinearSegments - s + 1);
        for(i = 0; i < Z_AXIS_ARRAY; i++) // End of segment in Cartesian steps
            // Perfect approximation, but slower, so we limit it to faster processors like arm
            destinationSteps[i] = static_cast<int32_t>(floor(0.5 + dx[i] * segment)) + Printer::currentPositionSteps[i];
#endif
        // Verify that delta calculation has a solution
        if (transformCartesianStepsToDeltaSteps(destinationSteps, destinationDeltaSteps)) {
            d->dir = 0;
#if DRIVE_SYSTEM == DELTA
            if (softEndstop) {
                destinationDeltaSteps[A_TOWER] = RMath::min(destinationDeltaSteps[A_TOWER], Printer::maxDeltaPositionSteps);
                destinationDeltaSteps[B_TOWER] = RMath::min(destinationDeltaSteps[B_TOWER], Printer::maxDeltaPositionSteps);
                destinationDeltaSteps[C_TOWER] = RMath::min(destinationDeltaSteps[C_TOWER], Printer::maxDeltaPositionSteps);
            }
#endif
            for(i = 0; i < TOWER_ARRAY; i++) {
                delta = destinationDeltaSteps[i] - Printer::currentNonlinearPositionSteps[i];
                if (delta > 0) {
                    d->setPositiveMoveOfAxis(i);
#ifdef DEBUG_DELTA_OVERFLOW
                    if (delta > 65535)
                        Com::printFLN(Com::tDBGDeltaOverflow, delta);
#endif
                    d->deltaSteps[i] = static_cast<uint16_t>(delta);
                } else {
                    d->setMoveOfAxis(i);
#ifdef DEBUG_DELTA_OVERFLOW
                    if (-delta > 65535)
                        Com::printFLN(Com::tDBGDeltaOverflow, delta);
#endif
                    d->deltaSteps[i] = static_cast<uint16_t>(-delta);
                }
#ifdef DEBUG_STEPCOUNT
                totalStepsRemaining += d->deltaSteps[i];
#endif
                if(d->deltaSteps[i] > maxAxisSteps)
                    maxAxisSteps = d->deltaSteps[i];
                Printer::currentNonlinearPositionSteps[i] = destinationDeltaSteps[i];
            }
        } else {
            // Illegal position - ignore move
            Com::printWarningF(Com::tInvalidDeltaCoordinate);
            Com::printF(PSTR(" x:"), destinationSteps[X_AXIS]);
            Com::printF(PSTR(" y:"), destinationSteps[Y_AXIS]);
            Com::printFLN(PSTR(" z:"), destinationSteps[Z_AXIS]);
            d->dir = 0;
            d->deltaSteps[A_TOWER] = d->deltaSteps[B_TOWER] = d->deltaSteps[C_TOWER] = 0;
            return 65535; // flag error
        }
    }
#ifdef DEBUG_STEPCOUNT
//      out.println_long_P(PSTR("initial StepsRemaining:"), p->totalStepsRemaining);
#endif
    return maxAxisSteps;
}

uint8_t PrintLine::calculateDistance(float axisDistanceMM[], uint8_t dir, float *distance) {
    // Calculate distance depending on direction
    if(dir & XYZ_STEP) {
        if(dir & XSTEP)
            *distance = axisDistanceMM[X_AXIS] * axisDistanceMM[X_AXIS];
        else
            *distance = 0;
        if(dir & YSTEP)
            *distance += axisDistanceMM[Y_AXIS] * axisDistanceMM[Y_AXIS];
        if(dir & ZSTEP)
            *distance += axisDistanceMM[Z_AXIS] * axisDistanceMM[Z_AXIS];
        *distance = RMath::max((float)sqrt(*distance), axisDistanceMM[E_AXIS]);
        return 1;
    } else {
        if(dir & ESTEP) {
            *distance = axisDistanceMM[E_AXIS];
            return 1;
        }
        *distance = 0;
        return 0;
    }
}

#if SOFTWARE_LEVELING
void PrintLine::calculatePlane(int32_t factors[], int32_t p1[], int32_t p2[], int32_t p3[]) {
    factors[0] = p1[1] * (p2[2] - p3[2]) + p2[1] * (p3[2] - p1[2]) + p3[1] * (p1[2] - p2[2]);
    factors[1] = p1[2] * (p2[0] - p3[0]) + p2[2] * (p3[0] - p1[0]) + p3[2] * (p1[0] - p2[0]);
    factors[2] = p1[0] * (p2[1] - p3[1]) + p2[0] * (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1]);
    factors[3] = p1[0] * ((p2[1] * p3[2]) - (p3[1] * p2[2])) + p2[0] * ((p3[1] * p1[2]) - (p1[1] * p3[2])) + p3[0] * ((p1[1] * p2[2]) - (p2[1] * p1[2]));
}

float PrintLine::calcZOffset(int32_t factors[], int32_t pointX, int32_t pointY) {
    return (factors[3] - factors[X_AXIS] * pointX - factors[Y_AXIS] * pointY) / (float) factors[2];
}
#endif

inline void PrintLine::queueEMove(int32_t extrudeDiff, uint8_t check_endstops, uint8_t pathOptimize) {
    Printer::unsetAllSteppersDisabled();
    waitForXFreeLines(1);
    uint8_t newPath = insertWaitMovesIfNeeded(pathOptimize, 1);
    PrintLine *p = getNextWriteLine();
    float axisDistanceMM[VIRTUAL_AXIS_ARRAY]; // Axis movement in mm
    if(check_endstops) p->flags = FLAG_CHECK_ENDSTOPS;
    else p->flags = 0;
#if MIXING_EXTRUDER
    if(Printer::isAllEMotors()) {
        p->flags |= FLAG_ALL_E_MOTORS;
    }
#endif
    p->joinFlags = 0;
    if(!pathOptimize) p->setEndSpeedFixed(true);
    //Find direction
    for(uint8_t i = 0; i < Z_AXIS_ARRAY; i++) {
        p->delta[i] = 0;
        axisDistanceMM[i] = 0;
    }
    if (extrudeDiff >= 0) {
        p->delta[E_AXIS] = extrudeDiff;
        p->dir = E_STEP_DIRPOS;
    } else {
        p->delta[E_AXIS] = -extrudeDiff;
        p->dir = ESTEP;
    }
    Printer::currentPositionSteps[E_AXIS] = Printer::destinationSteps[E_AXIS];

    p->numNonlinearSegments = 0;
    //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
    p->primaryAxis = E_AXIS;
    p->stepsRemaining = p->delta[E_AXIS];
    axisDistanceMM[E_AXIS] = p->distance = p->delta[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS];
    axisDistanceMM[VIRTUAL_AXIS] = -p->distance;
    p->moveID = lastMoveID++;
    p->calculateMove(axisDistanceMM, pathOptimize, E_AXIS);
}

/**
  Split a line up into a series of lines with at most DELTASEGMENTS_PER_PRINTLINE delta segments.
  @param check_endstops Check endstops during the move.
  @param pathOptimize Run the path optimizer.
  @param softEndstop check if we go out of bounds.
*/
uint8_t PrintLine::queueNonlinearMove(uint8_t check_endstops, uint8_t pathOptimize, uint8_t softEndstop) {
	ENSURE_POWER
    //if (softEndstop && Printer::destinationSteps[Z_AXIS] < 0) Printer::destinationSteps[Z_AXIS] = 0; // now constrained at entry level including cylinder test
    EVENT_CONTRAIN_DESTINATION_COORDINATES
    int32_t difference[E_AXIS_ARRAY];
    float axisDistanceMM[VIRTUAL_AXIS_ARRAY]; // Real cartesian axis movement in mm. Virtual axis in 4;
    secondspeed_t secondSpeed = Printer::fanSpeed;
    for(fast8_t axis = 0; axis < E_AXIS_ARRAY; axis++) {
        difference[axis] = Printer::destinationSteps[axis] - Printer::currentPositionSteps[axis];
        if(axis == E_AXIS) {
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::extrudeMultiplyError += (static_cast<float>(difference[E_AXIS]) * Printer::extrusionFactor);
                difference[E_AXIS] = static_cast<int32_t>(Printer::extrudeMultiplyError);
                Printer::extrudeMultiplyError -= difference[E_AXIS];
                axisDistanceMM[E_AXIS] = difference[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS];
                Printer::filamentPrinted += axisDistanceMM[E_AXIS];
                axisDistanceMM[E_AXIS] = fabs(axisDistanceMM[E_AXIS]);
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) {
                secondSpeed = ((axisDistanceMM[X_AXIS] != 0 || axisDistanceMM[Y_AXIS] != 0) && (LaserDriver::laserOn || axisDistanceMM[E_AXIS] != 0) ? LaserDriver::intensity : 0);
                axisDistanceMM[E_AXIS] = 0;
            }
#endif
        } else
            axisDistanceMM[axis] = fabs(difference[axis] * Printer::invAxisStepsPerMM[axis]);
    }

    float cartesianDistance;
    ufast8_t cartesianDir;
    int32_t cartesianDeltaSteps[E_AXIS_ARRAY];
    calculateDirectionAndDelta(difference, &cartesianDir, cartesianDeltaSteps);
    if (!calculateDistance(axisDistanceMM, cartesianDir, &cartesianDistance)) {
        // Appears the intent is to do nothing if no distance is detected.
        // This apparently is not an error condition, just early exit.
        return true;
    }

    if (!(cartesianDir & XYZ_STEP)) {
        queueEMove(difference[E_AXIS], check_endstops, pathOptimize);
        return true;
    }

    int16_t segmentCount;
#if DRIVE_SYSTEM == DELTA
    float feedrate = RMath::min(Printer::feedrate, Printer::maxFeedrate[Z_AXIS]);
#else
    float feedrate = Printer::feedrate; // each motor has own max. feedrate here resulting in total feedrate
#endif
    if (cartesianDir & XY_STEP) {
        // Compute number of seconds for move and hence number of segments needed
        //float seconds = 100 * cartesianDistance / (Printer::feedrate * Printer::feedrateMultiply); multiply in feedrate included
        float seconds = cartesianDistance / feedrate;
#ifdef DEBUG_SPLIT
        Com::printFLN(Com::tDBGDeltaSeconds, seconds);
#endif
        float sps = static_cast<float>((cartesianDir & ESTEP) == ESTEP ? Printer::printMovesPerSecond : Printer::travelMovesPerSecond);
        segmentCount = RMath::max(1, static_cast<int16_t>(sps * seconds));
#ifdef DEBUG_SEGMENT_LENGTH
        float segDist = cartesianDistance / (float)segmentCount;
        if(segDist > Printer::maxRealSegmentLength) {
            Printer::maxRealSegmentLength = segDist;
            Com::printFLN(PSTR("SegmentsPerSecond:"), sps);
            Com::printFLN(PSTR("New max. segment length:"), segDist);
        }
#endif
        //Com::printFLN(PSTR("Segments:"),segmentCount);
    } else {
        // Optimize pure Z axis move. Since a pure Z axis move is linear all we have to watch out for is unsigned integer overruns in
        // the queued moves;
#ifdef DEBUG_SPLIT
        Com::printFLN(Com::tDBGDeltaZDelta, cartesianDeltaSteps[Z_AXIS]);
#endif
        segmentCount = (cartesianDeltaSteps[Z_AXIS] + (uint32_t)43680) / (uint32_t)43679; // can not go to 65535 for rounding issues causing overflow later in some cases!
    }
    // Now compute the number of lines needed
    int numLines = (segmentCount + DELTASEGMENTS_PER_PRINTLINE - 1) / DELTASEGMENTS_PER_PRINTLINE;
    // There could be some error here but it doesn't matter since the number of segments will just be reduced slightly
    int segmentsPerLine = segmentCount / numLines;

    int32_t startPosition[E_AXIS_ARRAY], fractionalSteps[E_AXIS_ARRAY];
    if(numLines > 1) {
        for (fast8_t i = 0; i < Z_AXIS_ARRAY; i++)
            startPosition[i] = Printer::currentPositionSteps[i];
        startPosition[E_AXIS] = 0;
        cartesianDistance /= static_cast<float>(numLines);
    }

#ifdef DEBUG_SPLIT
    Com::printFLN(Com::tDBGDeltaSegments, segmentCount);
    Com::printFLN(Com::tDBGDeltaNumLines, numLines);
    Com::printFLN(Com::tDBGDeltaSegmentsPerLine, segmentsPerLine);
#endif
    Printer::unsetAllSteppersDisabled(); // Motor is enabled now
    waitForXFreeLines(1);

    // Insert dummy moves if necessary
    // Need to leave at least one slot open for the first split move
    insertWaitMovesIfNeeded(pathOptimize, RMath::min(PRINTLINE_CACHE_SIZE - 4, numLines));
    uint32_t oldEDestination = Printer::destinationSteps[E_AXIS]; // flow and volumetric extrusion changed virtual target
    Printer::currentPositionSteps[E_AXIS] = 0;

    for (int lineNumber = 1; lineNumber <= numLines; lineNumber++) {
        waitForXFreeLines(1);
        PrintLine *p = getNextWriteLine();
        // Downside a comparison per loop. Upside one less distance calculation and simpler code.
        if (numLines == 1) {
            // p->numDeltaSegments = segmentCount; // not neede, gets overwritten further down
            p->dir = cartesianDir;
            for (fast8_t i = 0; i < E_AXIS_ARRAY; i++) {
                p->delta[i] = cartesianDeltaSteps[i];
                fractionalSteps[i] = difference[i];
            }
            p->distance = cartesianDistance;
        } else {
            for (fast8_t i = 0; i < E_AXIS_ARRAY; i++) {
                Printer::destinationSteps[i] = startPosition[i] + (difference[i] * lineNumber) / numLines;
                fractionalSteps[i] = Printer::destinationSteps[i] - Printer::currentPositionSteps[i];
                axisDistanceMM[i] = fabs(fractionalSteps[i] * Printer::invAxisStepsPerMM[i]);
            }
            calculateDirectionAndDelta(fractionalSteps, &p->dir, p->delta);
            p->distance = cartesianDistance;
        }

        p->joinFlags = 0;
        p->secondSpeed = secondSpeed;
        p->moveID = lastMoveID;

        // Only set fixed on last segment
        if (lineNumber == numLines && !pathOptimize)
            p->setEndSpeedFixed(true);

        p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
#if MIXING_EXTRUDER
        if(Printer::isAllEMotors()) {
            p->flags |= FLAG_ALL_E_MOTORS;
        }
#endif
        p->numNonlinearSegments = segmentsPerLine;

        uint16_t maxStepsPerSegment = p->calculateNonlinearSubSegments(softEndstop);
        if (maxStepsPerSegment == 65535) {
            Com::printWarningFLN(PSTR("in queueDeltaMove to calculateDeltaSubSegments returns error."));
            return false;
        }
#ifdef DEBUG_SPLIT
        Com::printFLN(Com::tDBGDeltaMaxDS, (int32_t)maxStepsPerSegment);
#endif
        int32_t virtualAxisSteps = static_cast<int32_t>(maxStepsPerSegment) * segmentsPerLine;
        if (virtualAxisSteps == 0 && p->delta[E_AXIS] == 0) {
            if (numLines != 1) {
                Com::printErrorFLN(Com::tDBGDeltaNoMoveinDSegment);
                return false;  // Line too short in low precision area
            }
			break;
        }
        fast8_t drivingAxis = X_AXIS;
        p->primaryAxis = VIRTUAL_AXIS; // Virtual axis will lead Bresenham step either way
        if (virtualAxisSteps > p->delta[E_AXIS]) { // Is delta move or E axis leading
            p->stepsRemaining = virtualAxisSteps;
            axisDistanceMM[VIRTUAL_AXIS] = p->distance;  //virtual_axis_move * Printer::invAxisStepsPerMM[Z_AXIS]; // Steps/unit same as all the towers
            // Virtual axis steps per segment
            p->numPrimaryStepPerSegment = maxStepsPerSegment;
#if DRIVE_SYSTEM != DELTA
            if(cartesianDeltaSteps[Z_AXIS] > cartesianDeltaSteps[X_AXIS] && cartesianDeltaSteps[Z_AXIS] > cartesianDeltaSteps[Y_AXIS])
                drivingAxis = Z_AXIS;
#endif
        } else {
            // Round up the E move to get something divisible by segment count which is greater than E move
            p->numPrimaryStepPerSegment = (p->delta[E_AXIS] + segmentsPerLine - 1) / segmentsPerLine;
            p->stepsRemaining = p->numPrimaryStepPerSegment * segmentsPerLine;
            axisDistanceMM[VIRTUAL_AXIS] = -p->distance; //p->stepsRemaining * Printer::invAxisStepsPerMM[Z_AXIS];
            drivingAxis = E_AXIS;
        }
#ifdef DEBUG_SPLIT
        Com::printFLN(Com::tDBGDeltaStepsPerSegment, p->numPrimaryStepPerSegment);
        Com::printFLN(Com::tDBGDeltaVirtualAxisSteps, p->stepsRemaining);
#endif
        p->calculateMove(axisDistanceMM, pathOptimize, drivingAxis);
        for (fast8_t i = 0; i < E_AXIS_ARRAY; i++) {
            Printer::currentPositionSteps[i] += fractionalSteps[i];
        }
    }
    Printer::currentPositionSteps[E_AXIS] = Printer::destinationSteps[E_AXIS] = oldEDestination;
    lastMoveID++; // Will wrap at 255

    return true; // flag success
}

#endif

#if ARC_SUPPORT
// Arc function taken from grbl
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
// segment is configured in settings.mm_per_arc_segment.
void PrintLine::arc(float *position, float *target, float *offset, float radius, uint8_t isclockwise) {
    //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
    //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
    float center_axis0 = position[X_AXIS] + offset[X_AXIS];
    float center_axis1 = position[Y_AXIS] + offset[Y_AXIS];
    //float linear_travel = 0; //target[axis_linear] - position[axis_linear];
    float extruder_travel = (Printer::destinationSteps[E_AXIS] - Printer::currentPositionSteps[E_AXIS]) * Printer::invAxisStepsPerMM[E_AXIS];
    float r_axis0 = -offset[0];  // Radius vector from center to current location
    float r_axis1 = -offset[1];
    float rt_axis0 = target[0] - center_axis0;
    float rt_axis1 = target[1] - center_axis1;
    /*long xtarget = Printer::destinationSteps[X_AXIS];
    long ytarget = Printer::destinationSteps[Y_AXIS];
    long ztarget = Printer::destinationSteps[Z_AXIS];
    long etarget = Printer::destinationSteps[E_AXIS];
    */
    // CCW angle between position and target from circle center. Only one atan2() trig computation required.
    float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
    if ((!isclockwise && angular_travel <= 0.00001) || (isclockwise && angular_travel < -0.000001)) {
        angular_travel += 2.0f * M_PI;
    }
    if (isclockwise) {
        angular_travel -= 2.0f * M_PI;
    }

    float millimeters_of_travel = fabs(angular_travel) * radius; //hypot(angular_travel*radius, fabs(linear_travel));
    if (millimeters_of_travel < 0.001f) {
        return;// treat as succes because there is nothing to do;
    }
    //uint16_t segments = (radius>=BIG_ARC_RADIUS ? floor(millimeters_of_travel/MM_PER_ARC_SEGMENT_BIG) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT));
    // Increase segment size if printing faster then computation speed allows
    uint16_t segments = (Printer::feedrate > 60.0f ? floor(millimeters_of_travel / RMath::min(static_cast<float>(MM_PER_ARC_SEGMENT_BIG), Printer::feedrate * 0.01666f * static_cast<float>(MM_PER_ARC_SEGMENT))) : floor(millimeters_of_travel / static_cast<float>(MM_PER_ARC_SEGMENT)));
    if(segments == 0) segments = 1;
    /*
      // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
      // by a number of discrete segments. The inverse feed_rate should be correct for the sum of
      // all segments.
      if (invert_feed_rate) { feed_rate *= segments; }
    */
    float theta_per_segment = angular_travel / segments;
    //float linear_per_segment = linear_travel/segments;
    float extruder_per_segment = extruder_travel / segments;

    /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
       and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
           r_T = [cos(phi) -sin(phi);
                  sin(phi)  cos(phi] * r ;

       For arc generation, the center of the circle is the axis of rotation and the radius vector is
       defined from the circle center to the initial position. Each line segment is formed by successive
       vector rotations. This requires only two cos() and sin() computations to form the rotation
       matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
       all double numbers are single precision on the Arduino. (True double precision will not have
       round off issues for CNC applications.) Single precision error can accumulate to be greater than
       tool precision in some cases. Therefore, arc path correction is implemented.

       Small angle approximation may be used to reduce computation overhead further. This approximation
       holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
       theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
       to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
       numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
       issue for CNC machines with the single precision Arduino calculations.

       This approximation also allows mc_arc to immediately insert a line segment into the planner
       without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
       a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
       This is important when there are successive arc motions.
    */
    // Vector rotation matrix values
    float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
    float sin_T = theta_per_segment;

    float arc_target[4];
    float sin_Ti;
    float cos_Ti;
    float r_axisi;
    uint16_t i;
    int8_t count = 0;

    // Initialize the linear axis
    //arc_target[axis_linear] = position[axis_linear];

    // Initialize the extruder axis
    arc_target[E_AXIS] = Printer::currentPositionSteps[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS];

    for (i = 1; i < segments; i++) {
        // Increment (segments-1)

        if((count & 3) == 0) {
            //GCode::readFromSerial();
            Commands::checkForPeriodicalActions(false);
            UI_MEDIUM; // do check encoder
        }

        if (count < N_ARC_CORRECTION) { //25 pieces
            // Apply vector rotation matrix
            r_axisi = r_axis0 * sin_T + r_axis1 * cos_T;
            r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T;
            r_axis1 = r_axisi;
            count++;
        } else {
            // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
            // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
            cos_Ti  = cos(i * theta_per_segment);
            sin_Ti  = sin(i * theta_per_segment);
            r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti;
            r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti;
            count = 0;
        }

        // Update arc_target location
        arc_target[X_AXIS] = center_axis0 + r_axis0;
        arc_target[Y_AXIS] = center_axis1 + r_axis1;
        //arc_target[axis_linear] += linear_per_segment;
        arc_target[E_AXIS] += extruder_per_segment;
        Printer::moveToReal(arc_target[X_AXIS], arc_target[Y_AXIS], IGNORE_COORDINATE, arc_target[E_AXIS], IGNORE_COORDINATE);
    }
    // Ensure last segment arrives at target location.
    Printer::moveToReal(target[X_AXIS], target[Y_AXIS], IGNORE_COORDINATE, target[E_AXIS], IGNORE_COORDINATE);
}
#endif



/**
  Moves the stepper motors one step. If the last step is reached, the next movement is started.
  The function must be called from a timer loop. It returns the time for the next call.
  This is a modified version that implements a Bresenham 'multi-step' algorithm where the dominant
  Cartesian axis steps may be less than the changing dominant delta axis.
*/
#if NONLINEAR_SYSTEM
int lastblk = - 1;
int32_t cur_errupd;
// Current nonlinear segment
NonlinearSegment *curd;
// Current nonlinear segment primary error increment
int32_t curd_errupd, stepsPerSegRemaining;
int32_t PrintLine::bresenhamStep() { // Version for delta printer
#if CPU_ARCH == ARCH_ARM
    if(!PrintLine::nlFlag)
#else
    if(cur == NULL)
#endif
    {
        setCurrentLine();
        if(cur->isBlocked()) { // This step is in computation - shouldn't happen
            if(lastblk != (int)cur) {
                HAL::allowInterrupts();
                lastblk = (int)cur;
                Com::printFLN(Com::tBLK, (int32_t)linesCount);
            }
            cur = NULL;
#if CPU_ARCH == ARCH_ARM
            PrintLine::nlFlag = false;
#endif
            return 2000;
        }
        HAL::allowInterrupts();
        lastblk = -1;
#if INCLUDE_DEBUG_NO_MOVE
        if(Printer::debugNoMoves()) { // simulate a move, but do nothing in reality
            removeCurrentLineForbidInterrupt();
            if(linesCount == 0) UI_STATUS_F(Com::translatedF(UI_TEXT_IDLE_ID));
            return 1000;
        }
#endif
        if(cur->isWarmUp()) {
            // This is a warm up move to initialize the path planner correctly. Just waste
            // a bit of time to get the planning up to date.
            if(linesCount <= cur->getWaitForXLinesFilled()) {
                cur = NULL;
#if CPU_ARCH==ARCH_ARM
                PrintLine::nlFlag = false;
#endif
                return 2000;
            }
#if LASER_WARMUP_TIME > 0 && SUPPORT_LASER
            if(cur->dir) {
                LaserDriver::changeIntensity(LASER_PWM_MAX);
            }
#endif
            long wait = cur->getWaitTicks();
            removeCurrentLineForbidInterrupt();
            return(wait); // waste some time for path optimization to fill up
        } // End if WARMUP
#if FEATURE_Z_PROBE
        // z move may consist of more then 1 z line segment, so we better ignore them
        // if the probe was already hit.
        if(Printer::isZProbingActive() && Printer::stepsRemainingAtZHit >= 0) {
            removeCurrentLineForbidInterrupt();
            if(linesCount == 0) UI_STATUS_F(Com::translatedF(UI_TEXT_IDLE_ID));
            return 1000;
        }
#endif

        if(cur->isEMove()) {
            Extruder::enable();
        }
        cur->fixStartAndEndSpeed();
        // Set up delta segments
        if (cur->numNonlinearSegments) {

            // If there are delta segments point to them here
            curd = &cur->segments[--cur->numNonlinearSegments];
            // Enable axis - All axis are enabled since they will most probably all be involved in a move
            // Since segments could involve different axis this reduces load when switching segments and
            // makes disabling easier.
            Printer::enableXStepper();
            Printer::enableYStepper();
            Printer::enableZStepper();
            Printer::setXDirection(curd->isXPositiveMove());
            Printer::setYDirection(curd->isYPositiveMove());
            Printer::setZDirection(curd->isZPositiveMove());

            // Copy across movement into main direction flags so that endstops function correctly
#if DRIVE_SYSTEM == DELTA
            cur->dir |= curd->dir; // deltas need this for homing!
#endif
            // Initialize Bresenham for the first segment
            cur->error[X_AXIS] = cur->error[Y_AXIS] = cur->error[Z_AXIS] = cur->numPrimaryStepPerSegment >> 1;
            curd_errupd = cur->numPrimaryStepPerSegment;
            stepsPerSegRemaining = cur->numPrimaryStepPerSegment;
        } else curd = NULL;
        cur_errupd = cur->stepsRemaining;

        if(!cur->areParameterUpToDate()) { // should never happen, but with bad timings???
            cur->updateStepsParameter();
        }
        Printer::vMaxReached = cur->vStart;
        Printer::stepNumber = 0;
        Printer::timer = 0;
        HAL::forbidInterrupts();
#if USE_ADVANCE
        if(!Printer::isAdvanceActivated()) // Set direction if no advance/OPS enabled
#endif
            Extruder::setDirection(cur->isEPositiveMove());
#if defined(DIRECTION_DELAY) && DIRECTION_DELAY > 0
        // HAL::delayMicroseconds(DIRECTION_DELAY); // We leave interrupt without step so no delay needed here
#endif
#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
        Printer::advanceExecuted = cur->advanceStart;
#endif
        cur->updateAdvanceSteps(cur->vStart, 0, false);
#endif
        if(Printer::mode == PRINTER_MODE_FFF) {
            Printer::setFanSpeedDirectly(cur->secondSpeed);
        }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
        else if(Printer::mode == PRINTER_MODE_LASER) {
            LaserDriver::changeIntensity(cur->secondSpeed);
        }
#endif
#if MULTI_XENDSTOP_HOMING
		Printer::multiXHomeFlags = MULTI_XENDSTOP_ALL;  // move all x motors until endstop says differently
#endif
#if MULTI_YENDSTOP_HOMING
		Printer::multiYHomeFlags = MULTI_YENDSTOP_ALL;  // move all y motors until endstop says differently
#endif
#if MULTI_ZENDSTOP_HOMING
		Printer::multiZHomeFlags = MULTI_ZENDSTOP_ALL;  // move all z motors until endstop says differently
#endif
        return Printer::interval; // Wait an other 50% from last step to make the 100% full
    } // End cur=0
    HAL::allowInterrupts();

    if(curd != NULL) {
        if(curd->checkEndstops(cur, (cur->isCheckEndstops()))) { // should stop move
            cur->stepsRemaining = 0;
            curd = NULL;
            // eat up all following segments with moveID
            uint8_t delId = cur->moveID;
            removeCurrentLineForbidInterrupt();
            while(linesCount > 0) {
                setCurrentLine();
                if(cur->isBlocked() || cur->moveID != delId) {
                    break;
                }
                removeCurrentLineForbidInterrupt();
            }
            cur = NULL;
#if CPU_ARCH == ARCH_ARM
            nlFlag = false;
#endif
            Printer::disableAllowedStepper();
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::setFanSpeedDirectly(Printer::fanSpeed);
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) { // Last move disables laser for safety!
                LaserDriver::changeIntensity(0);
            }
#endif
            return Printer::interval;
        }
    }
    int maxLoops = (Printer::stepsPerTimerCall <= cur->stepsRemaining ? Printer::stepsPerTimerCall : cur->stepsRemaining);
    HAL::forbidInterrupts();
    for(int loop = 0; loop < maxLoops; loop++) {
#if STEPPER_HIGH_DELAY + DOUBLE_STEP_DELAY
        if(loop > 0)
            HAL::delayMicroseconds(STEPPER_HIGH_DELAY + DOUBLE_STEP_DELAY);
#endif
        if((cur->error[E_AXIS] -= cur->delta[E_AXIS]) < 0) {
#if USE_ADVANCE
            if(Printer::isAdvanceActivated()) { // Use interrupt for movement
                if(cur->isEPositiveMove())
                    Printer::extruderStepsNeeded++;
                else
                    Printer::extruderStepsNeeded--;
            } else
#endif
                Extruder::step();
            cur->error[E_AXIS] += cur_errupd;
        }
        if (curd) {
            // Take delta steps
            if(curd->isXMove())
                if((cur->error[X_AXIS] -= curd->deltaSteps[A_TOWER]) < 0) {
                    cur->startXStep();
                    cur->error[X_AXIS] += curd_errupd;
#ifdef DEBUG_REAL_POSITION
                    Printer::realDeltaPositionSteps[A_TOWER] += curd->isXPositiveMove() ? 1 : -1;
#endif
#ifdef DEBUG_STEPCOUNT
                    cur->totalStepsRemaining--;
#endif
                }

            if(curd->isYMove())
                if((cur->error[Y_AXIS] -= curd->deltaSteps[B_TOWER]) < 0) {
                    cur->startYStep();
                    cur->error[Y_AXIS] += curd_errupd;
#ifdef DEBUG_REAL_POSITION
                    Printer::realDeltaPositionSteps[B_TOWER] += curd->isYPositiveMove() ? 1 : -1;
#endif
#ifdef DEBUG_STEPCOUNT
                    cur->totalStepsRemaining--;
#endif
                }

            if(curd->isZMove())
                if((cur->error[Z_AXIS] -= curd->deltaSteps[C_TOWER]) < 0) {
                    cur->startZStep();
                    cur->error[Z_AXIS] += curd_errupd;
                    Printer::realDeltaPositionSteps[C_TOWER] += curd->isZPositiveMove() ? 1 : -1;
#ifdef DEBUG_STEPCOUNT
                    cur->totalStepsRemaining--;
#endif
                }
            stepsPerSegRemaining--;
        }
#if CPU_ARCH != ARCH_AVR
        if(loop < maxLoops - 1) {
#endif
            Printer::insertStepperHighDelay();
            Printer::endXYZSteps();
#if USE_ADVANCE
            if(!Printer::isAdvanceActivated()) // Use interrupt for movement
#endif
                Extruder::unstep();
#if CPU_ARCH != ARCH_AVR
        }
#endif
        if (!stepsPerSegRemaining) { // start new nonlinear segment
            if (cur->numNonlinearSegments && curd != NULL) {
#if FEATURE_BABYSTEPPING
                if(Printer::zBabystepsMissing/* && curd
                            && (curd->dir & XYZ_STEP) == XYZ_STEP*/) {
                    // execute a extra baby step
                    Printer::zBabystep();
                }
#endif
                // Get the next delta segment
                curd = &cur->segments[--cur->numNonlinearSegments];

                // Initialize Bresenham for this segment (numPrimaryStepPerSegment is already correct for the half step setting)
                cur->error[X_AXIS] = cur->error[Y_AXIS] = cur->error[Z_AXIS] = cur->numPrimaryStepPerSegment >> 1;

                // Reset the counter of the primary steps. This is initialized in the line
                // generation so don't have to do this the first time.
                stepsPerSegRemaining = cur->numPrimaryStepPerSegment;

                // Change direction if necessary
                Printer::setXDirection(curd->dir & X_DIRPOS);
                Printer::setYDirection(curd->dir & Y_DIRPOS);
                Printer::setZDirection(curd->dir & Z_DIRPOS);
#if defined(DIRECTION_DELAY) && DIRECTION_DELAY > 0
#if CPU_ARCH != ARCH_AVR
                if(loop < maxLoops - 1)
#endif
                    HAL::delayMicroseconds(DIRECTION_DELAY);
#endif

            } else
                curd = 0;// Release the last segment
            //deltaSegmentCount--;
        }
    } // for loop

    HAL::allowInterrupts(); // Allow interrupts for other types, timer1 is still disabled
#if RAMP_ACCELERATION
//If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval
    if (cur->moveAccelerating()) {
        Printer::vMaxReached = HAL::ComputeV(Printer::timer, cur->fAcceleration) + cur->vStart;
        if(Printer::vMaxReached > cur->vMax) Printer::vMaxReached = cur->vMax;
        speed_t v = Printer::updateStepsPerTimerCall(Printer::vMaxReached);
        Printer::interval = HAL::CPUDivU2(v);
        // if(Printer::maxInterval < Printer::interval) // fix timing for very slow speeds
        //    Printer::interval = Printer::maxInterval;
        Printer::timer += Printer::interval;
        cur->updateAdvanceSteps(Printer::vMaxReached, maxLoops, true);
        Printer::stepNumber += maxLoops; // is only used by moveAccelerating
    } else if (cur->moveDecelerating()) { // time to slow down
        speed_t v = HAL::ComputeV(Printer::timer, cur->fAcceleration);
        if (v > Printer::vMaxReached)   // if deceleration goes too far it can become too large
            v = cur->vEnd;
        else {
            v = Printer::vMaxReached - v;
            if (v < cur->vEnd) v = cur->vEnd; // extra steps at the end of deceleration due to rounding errors
        }
        cur->updateAdvanceSteps(v, maxLoops, false);
        v = Printer::updateStepsPerTimerCall(v);
        Printer::interval = HAL::CPUDivU2(v);
        // if(Printer::maxInterval < Printer::interval) // fix timing for very slow speeds
        //    Printer::interval = Printer::maxInterval;
        Printer::timer += Printer::interval;
    } else {
        // If we had acceleration, we need to use the latest vMaxReached and interval
        // If we started full speed, we need to use cur->fullInterval and vMax
        cur->updateAdvanceSteps((!cur->accelSteps ? cur->vMax : Printer::vMaxReached), 0, true);
        if(!cur->accelSteps) {
            if(cur->vMax > STEP_DOUBLER_FREQUENCY) {
#if ALLOW_QUADSTEPPING
                if(cur->vMax > STEP_DOUBLER_FREQUENCY * 2) {
                    Printer::stepsPerTimerCall = 4;
                    Printer::interval = cur->fullInterval << 2;
                } else {
                    Printer::stepsPerTimerCall = 2;
                    Printer::interval = cur->fullInterval << 1;
                }
#else
                Printer::stepsPerTimerCall = 2;
                Printer::interval = cur->fullInterval << 1;
#endif
            } else {
                Printer::stepsPerTimerCall = 1;
                Printer::interval = cur->fullInterval;
            }
        }
    }
#else
    Printer::interval = cur->fullInterval; // without RAMPS always use full speed
#endif
    PrintLine::cur->stepsRemaining -= maxLoops;

    if(cur->stepsRemaining <= 0 || cur->isNoMove()) { // line finished
        // Release remaining delta segments
#ifdef DEBUG_STEPCOUNT
        if(cur->totalStepsRemaining || cur->numNonlinearSegments) {
            Com::printFLN(PSTR("Missed steps:"), cur->totalStepsRemaining);
            Com::printFLN(PSTR("Step/seg r:"), stepsPerSegRemaining);
            Com::printFLN(PSTR("NDS:"), (int) cur->numNonlinearSegments);
        }
#endif
        removeCurrentLineForbidInterrupt();
        Printer::disableAllowedStepper();
        if(linesCount == 0) {
            if(!Printer::isPrinting()) {
                UI_STATUS_F(Com::translatedF(UI_TEXT_IDLE_ID));
            }
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::setFanSpeedDirectly(Printer::fanSpeed);
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) { // Last move disables laser for safety!
                LaserDriver::changeIntensity(0);
            }
#endif
        }
        Printer::interval >>= 1; // 50% of time to next call to do cur=0
        DEBUG_MEMORY;
    } // Do even
#if CPU_ARCH != ARCH_AVR
    Printer::insertStepperHighDelay();
    Printer::endXYZSteps();
#if USE_ADVANCE
    if(!Printer::isAdvanceActivated()) // Use interrupt for movement
#endif
        Extruder::unstep();
#endif
    return Printer::interval;
}
#else
/**
  Moves the stepper motors one step. If the last step is reached, the next movement is started.
  The function must be called from a timer loop. It returns the time for the next call.

  Normal linear algorithm
*/
int lastblk = -1;
int32_t cur_errupd;
int32_t PrintLine::bresenhamStep() { // version for Cartesian printer
#if CPU_ARCH == ARCH_ARM
    if(!PrintLine::nlFlag)
#else
    if(cur == NULL)
#endif
    {
        setCurrentLine();
        if(cur->isBlocked()) { // This step is in computation - shouldn't happen
            /*if(lastblk!=(int)cur) // can cause output errors!
            {
                HAL::allowInterrupts();
                lastblk = (int)cur;
                Com::printFLN(Com::tBLK,lines_count);
            }*/
            cur = NULL;
#if CPU_ARCH==ARCH_ARM
            PrintLine::nlFlag = false;
#endif
            return 2000;
        }
        HAL::allowInterrupts();
        lastblk = -1;
#if INCLUDE_DEBUG_NO_MOVE
        if(Printer::debugNoMoves()) { // simulate a move, but do nothing in reality
            removeCurrentLineForbidInterrupt();
            return 1000;
        }
#endif
        ANALYZER_OFF(ANALYZER_CH0);
        if(cur->isWarmUp()) {
            // This is a warm up move to initialize the path planner correctly. Just waste
            // a bit of time to get the planning up to date.
            if(linesCount <= cur->getWaitForXLinesFilled()) {
                cur = NULL;
#if CPU_ARCH == ARCH_ARM
                PrintLine::nlFlag = false;
#endif
                return 2000;
            }
#if LASER_WARMUP_TIME > 0 && SUPPORT_LASER
            if(cur->dir) {
                LaserDriver::changeIntensity(LASER_PWM_MAX);
            }
#endif
            long wait = cur->getWaitTicks();
            removeCurrentLineForbidInterrupt();
            return(wait); // waste some time for path optimization to fill up
        } // End if WARMUP
        //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration.
#if GANTRY
#if DRIVE_SYSTEM == XY_GANTRY || DRIVE_SYSTEM == YX_GANTRY
        if(cur->isXOrYMove()) {
            Printer::enableXStepper();
            Printer::enableYStepper();
        }
        if(cur->isZMove()) Printer::enableZStepper();
#else // XZ / ZX Gantry
        if(cur->isXOrZMove()) {
            Printer::enableXStepper();
            Printer::enableZStepper();
        }
        if(cur->isYMove()) Printer::enableYStepper();
#endif
#else
        if(cur->isXMove()) Printer::enableXStepper();
        if(cur->isYMove()) Printer::enableYStepper();
        if(cur->isZMove()) Printer::enableZStepper();
#endif
        if(cur->isEMove()) Extruder::enable();
        cur->fixStartAndEndSpeed();
        HAL::allowInterrupts();
        cur_errupd = cur->delta[cur->primaryAxis];
        if(!cur->areParameterUpToDate()) { // should never happen, but with bad timings???
            cur->updateStepsParameter();
        }
        Printer::vMaxReached = cur->vStart;
        Printer::stepNumber = 0;
        Printer::timer = 0;
        HAL::forbidInterrupts();
        //Determine direction of movement,check if endstop was hit
#if !(GANTRY)
        Printer::setXDirection(cur->isXPositiveMove());
        Printer::setYDirection(cur->isYPositiveMove());
        Printer::setZDirection(cur->isZPositiveMove());
#else // Any gantry type
        long gdx = (cur->dir & X_DIRPOS ? cur->delta[X_AXIS] : -cur->delta[X_AXIS]); // Compute signed difference in steps
#if DRIVE_SYSTEM == XY_GANTRY || DRIVE_SYSTEM == YX_GANTRY
        Printer::setZDirection(cur->isZPositiveMove());
        long gdy = (cur->dir & Y_DIRPOS ? cur->delta[Y_AXIS] : -cur->delta[Y_AXIS]);
        Printer::setXDirection(gdx + gdy >= 0);
#if DRIVE_SYSTEM == XY_GANTRY
        Printer::setYDirection(gdx > gdy);
#else
        Printer::setYDirection(gdx <= gdy);
#endif
#else // XZ or ZX core
        Printer::setYDirection(cur->isYPositiveMove());
        long gdz = (cur->dir & Z_DIRPOS ? cur->delta[Z_AXIS] : -cur->delta[Z_AXIS]);
        Printer::setXDirection(gdx + gdz >= 0);
#if DRIVE_SYSTEM == XZ_GANTRY
        Printer::setZDirection(gdx > gdz);
#else
        Printer::setZDirection(gdx <= gdz);
#endif
#endif // YZ or ZY Gantry
#endif // GANTRY
#if USE_ADVANCE
        if(!Printer::isAdvanceActivated()) // Set direction if no advance/OPS enabled
#endif
            Extruder::setDirection(cur->isEPositiveMove());
#if defined(DIRECTION_DELAY) && DIRECTION_DELAY > 0
        // HAL::delayMicroseconds(DIRECTION_DELAY); // We leave interrupt without step so no delay needed here
#endif
#if USE_ADVANCE
#if ENABLE_QUADRATIC_ADVANCE
        Printer::advanceExecuted = cur->advanceStart;
#endif
        cur->updateAdvanceSteps(cur->vStart, 0, false);
#endif
        if(Printer::mode == PRINTER_MODE_FFF) {
            Printer::setFanSpeedDirectly(static_cast<uint8_t>(cur->secondSpeed));
        }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
        else if(Printer::mode == PRINTER_MODE_LASER) {
            LaserDriver::changeIntensity(cur->secondSpeed);
        }
#endif
#if MULTI_XENDSTOP_HOMING
		Printer::multiXHomeFlags = MULTI_XENDSTOP_ALL;  // move all x motors until endstop says differently
#endif
#if MULTI_YENDSTOP_HOMING
		Printer::multiYHomeFlags = MULTI_YENDSTOP_ALL;  // move all y motors until endstop says differently
#endif
#if MULTI_ZENDSTOP_HOMING
        Printer::multiZHomeFlags = MULTI_ZENDSTOP_ALL;  // move all z motors until endstop says differently
#endif
        return Printer::interval; // Wait an other 50% from last step to make the 100% full
    } // End cur=0
    cur->checkEndstops();
    fast8_t max_loops = Printer::stepsPerTimerCall;
    if(cur->stepsRemaining < max_loops)
        max_loops = cur->stepsRemaining;
    for(fast8_t loop = 0; loop < max_loops; loop++) {
#if STEPPER_HIGH_DELAY + DOUBLE_STEP_DELAY > 0
        if(loop)
            HAL::delayMicroseconds(STEPPER_HIGH_DELAY + DOUBLE_STEP_DELAY);
#endif
        if((cur->error[E_AXIS] -= cur->delta[E_AXIS]) < 0) {
#if USE_ADVANCE
            if(Printer::isAdvanceActivated()) { // Use interrupt for movement
                if(cur->isEPositiveMove())
                    Printer::extruderStepsNeeded++;
                else
                    Printer::extruderStepsNeeded--;
            } else
#endif
                Extruder::step();
            cur->error[E_AXIS] += cur_errupd;
        }
#if CPU_ARCH == ARCH_AVR
        if(cur->isXMove())
#endif
            if((cur->error[X_AXIS] -= cur->delta[X_AXIS]) < 0) {
                cur->startXStep();
                cur->error[X_AXIS] += cur_errupd;
            }
#if CPU_ARCH == ARCH_AVR
        if(cur->isYMove())
#endif
            if((cur->error[Y_AXIS] -= cur->delta[Y_AXIS]) < 0) {
                cur->startYStep();
                cur->error[Y_AXIS] += cur_errupd;
            }
#if CPU_ARCH == ARCH_AVR
        if(cur->isZMove())
#endif
            if((cur->error[Z_AXIS] -= cur->delta[Z_AXIS]) < 0) {
                cur->startZStep();
                cur->error[Z_AXIS] += cur_errupd;
#ifdef DEBUG_STEPCOUNT
                cur->totalStepsRemaining--;
#endif
            }
#if (GANTRY)
#if DRIVE_SYSTEM == XY_GANTRY || DRIVE_SYSTEM == YX_GANTRY
        Printer::executeXYGantrySteps();
#else
        Printer::executeXZGantrySteps();
#endif
#endif
        Printer::insertStepperHighDelay();
#if USE_ADVANCE
        if(!Printer::isAdvanceActivated()) // Use interrupt for movement
#endif
            Extruder::unstep();
        cur->stepsRemaining--;
        Printer::endXYZSteps();
    } // for loop
    HAL::allowInterrupts(); // Allow interrupts for other types, timer1 is still disabled
#if RAMP_ACCELERATION
    //If acceleration is enabled on this move and we are in the acceleration segment, calculate the current interval
    if (cur->moveAccelerating()) { // we are accelerating
        Printer::vMaxReached = HAL::ComputeV(Printer::timer, cur->fAcceleration) + cur->vStart; // v = v0 + a * t
        if(Printer::vMaxReached > cur->vMax) Printer::vMaxReached = cur->vMax;
        unsigned int v = Printer::updateStepsPerTimerCall(Printer::vMaxReached);
        Printer::interval = HAL::CPUDivU2(v);
        // if(Printer::maxInterval < Printer::interval) // fix timing for very slow speeds
        //    Printer::interval = Printer::maxInterval;
        Printer::timer += Printer::interval;
        cur->updateAdvanceSteps(Printer::vMaxReached, max_loops, true);
        Printer::stepNumber += max_loops; // only used for moveAccelerating
    } else if (cur->moveDecelerating()) { // time to slow down
        unsigned int v = HAL::ComputeV(Printer::timer, cur->fAcceleration);
        if (v > Printer::vMaxReached)   // if deceleration goes too far it can become too large
            v = cur->vEnd;
        else {
            v = Printer::vMaxReached - v;
            if (v < cur->vEnd) v = cur->vEnd; // extra steps at the end of deceleration due to rounding errors
        }
        cur->updateAdvanceSteps(v, max_loops, false); // needs original v
        v = Printer::updateStepsPerTimerCall(v);
        Printer::interval = HAL::CPUDivU2(v);
        // if(Printer::maxInterval < Printer::interval) // fix timing for very slow speeds
        //    Printer::interval = Printer::maxInterval;
        Printer::timer += Printer::interval;
    } else { // full speed reached
        cur->updateAdvanceSteps((!cur->accelSteps ? cur->vMax : Printer::vMaxReached), 0, true);
        // constant speed reached
        if(cur->vMax > STEP_DOUBLER_FREQUENCY) {
#if ALLOW_QUADSTEPPING
            if(cur->vMax > STEP_DOUBLER_FREQUENCY * 2) {
                Printer::stepsPerTimerCall = 4;
                Printer::interval = cur->fullInterval << 2;
            } else {
                Printer::stepsPerTimerCall = 2;
                Printer::interval = cur->fullInterval << 1;
            }
#else
            Printer::stepsPerTimerCall = 2;
            Printer::interval = cur->fullInterval << 1;
#endif
        } else {
            Printer::stepsPerTimerCall = 1;
            Printer::interval = cur->fullInterval;
        }
    }
#else
    Printer::stepsPerTimerCall = 1;
    Printer::interval = cur->fullInterval; // without RAMPS always use full speed
#endif // RAMP_ACCELERATION    
    long interval = Printer::interval;
    if(cur->stepsRemaining <= 0 || cur->isNoMove()) { // line finished
#ifdef DEBUG_STEPCOUNT
        if(cur->totalStepsRemaining) {
            Com::printF(Com::tDBGMissedSteps, cur->totalStepsRemaining);
            Com::printFLN(Com::tComma, cur->stepsRemaining);
        }
#endif
        removeCurrentLineForbidInterrupt();
        Printer::disableAllowedStepper();
        if(linesCount == 0) {
            if(!Printer::isPrinting()) {
                UI_STATUS_F(Com::translatedF(UI_TEXT_IDLE_ID));
            }
            if(Printer::mode == PRINTER_MODE_FFF) {
                Printer::setFanSpeedDirectly(Printer::fanSpeed);
            }
#if defined(SUPPORT_LASER) && SUPPORT_LASER
            else if(Printer::mode == PRINTER_MODE_LASER) { // Last move disables laser for safety!
                LaserDriver::changeIntensity(0);
            }
#endif
        }
        interval = Printer::interval = interval >> 1; // 50% of time to next call to do cur=0
        DEBUG_MEMORY;
    } // Do even
#if FEATURE_BABYSTEPPING
    if(Printer::zBabystepsMissing) {
        HAL::forbidInterrupts();
        Printer::zBabystep();
    }
#endif
    return interval;
}
#endif