Arduino/Repetier/Printer.h

/*
    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.
*/

/**

Coordinate system transformations:

Level 1: G-code => Coordinates like send via g-codes.

Level 2: Real coordinates => Coordinates corrected by coordinate shift via G92
         currentPosition and lastCmdPos are from this level.
Level 3: Transformed and shifter => Include extruder offset and bed rotation.
         These variables are only stored temporary.

Level 4: Step position => Level 3 converted into steps for motor position
        currentPositionSteps and destinationPositionSteps are from this level.

Level 5: Nonlinear motor step position, only for nonlinear drive systems
         destinationDeltaSteps


*/

#ifndef PRINTER_H_INCLUDED
#define PRINTER_H_INCLUDED

union floatLong
{
    float f;
    uint32_t l;
#ifdef SUPPORT_64_BIT_MATH
    uint64_t L;
#endif
};

union wizardVar
{
    float f;
    int32_t l;
    uint32_t ul;
    int16_t i;
    uint16_t ui;
    int8_t c;
    uint8_t uc;

    wizardVar():i(0) {}
    wizardVar(float _f):f(_f) {}
    wizardVar(int32_t _f):l(_f) {}
    wizardVar(uint32_t _f):ul(_f) {}
    wizardVar(int16_t _f):i(_f) {}
    wizardVar(uint16_t _f):ui(_f) {}
    wizardVar(int8_t _f):c(_f) {}
    wizardVar(uint8_t _f):uc(_f) {}
};

#define PRINTER_FLAG0_STEPPER_DISABLED      1
#define PRINTER_FLAG0_SEPERATE_EXTRUDER_INT 2
#define PRINTER_FLAG0_TEMPSENSOR_DEFECT     4
#define PRINTER_FLAG0_FORCE_CHECKSUM        8
#define PRINTER_FLAG0_MANUAL_MOVE_MODE      16
#define PRINTER_FLAG0_AUTOLEVEL_ACTIVE      32
#define PRINTER_FLAG0_ZPROBEING             64
#define PRINTER_FLAG0_LARGE_MACHINE         128
#define PRINTER_FLAG1_HOMED                 1
#define PRINTER_FLAG1_AUTOMOUNT             2
#define PRINTER_FLAG1_ANIMATION             4
#define PRINTER_FLAG1_ALLKILLED             8
#define PRINTER_FLAG1_UI_ERROR_MESSAGE      16
#define PRINTER_FLAG1_NO_DESTINATION_CHECK  32
#define PRINTER_FLAG1_POWER_ON              64
#define PRINTER_FLAG1_ALLOW_COLD_EXTRUSION  128
#define PRINTER_FLAG2_BLOCK_RECEIVING       1
#define PRINTER_FLAG2_AUTORETRACT           2
#define PRINTER_FLAG2_RESET_FILAMENT_USAGE  4
#define PRINTER_FLAG2_IGNORE_M106_COMMAND   8
#define PRINTER_FLAG2_DEBUG_JAM             16
#define PRINTER_FLAG2_JAMCONTROL_DISABLED   32
#define PRINTER_FLAG2_HOMING                64
#define PRINTER_FLAG2_ALL_E_MOTORS          128 // Set all e motors flag

// List of possible interrupt events (1-255 allowed)
#define PRINTER_INTERRUPT_EVENT_JAM_DETECTED 1
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL0 2
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL1 3
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL2 4
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL3 5
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL4 6
#define PRINTER_INTERRUPT_EVENT_JAM_SIGNAL5 7
// define an integer number of steps more than large enough to get to endstop from anywhere
#define HOME_DISTANCE_STEPS (Printer::zMaxSteps-Printer::zMinSteps+1000)
#define HOME_DISTANCE_MM (HOME_DISTANCE_STEPS * invAxisStepsPerMM[Z_AXIS])
// Some defines to make clearer reading, as we overload these cartesian memory locations for delta
#define towerAMaxSteps Printer::xMaxSteps
#define towerBMaxSteps Printer::yMaxSteps
#define towerCMaxSteps Printer::zMaxSteps
#define towerAMinSteps Printer::xMinSteps
#define towerBMinSteps Printer::yMinSteps
#define towerCMinSteps Printer::zMinSteps

class Plane {
	public:
	// f(x, y) = ax + by + c
	float a,b,c;
	float z(float x,float y) {
		return a * x + y * b + c;
	}
};
#if DISTORTION_CORRECTION
class Distortion
{
public:
    Distortion();
    void init();
    void enable(bool permanent = true);
    void disable(bool permanent = true);
    bool measure(void);
    int32_t correct(int32_t x, int32_t y, int32_t z) const;
    void updateDerived();
    void reportStatus();
	bool isEnabled() {return enabled;}
	int32_t zMaxSteps() {return zEnd;}	
	void set(float x,float y,float z);
	void showMatrix();		
    void resetCorrection();
private:
    int matrixIndex(fast8_t x, fast8_t y) const;
    int32_t getMatrix(int index) const;
    void setMatrix(int32_t val, int index);
    bool isCorner(fast8_t i, fast8_t j) const;
    INLINE int32_t extrapolatePoint(fast8_t x1, fast8_t y1, fast8_t x2, fast8_t y2) const;
    void extrapolateCorner(fast8_t x, fast8_t y, fast8_t dx, fast8_t dy);
    void extrapolateCorners();
// attributes
#if DRIVE_SYSTEM == DELTA	
    int32_t step;
    int32_t radiusCorrectionSteps;
#else
	int32_t xCorrectionSteps,xOffsetSteps;
	int32_t yCorrectionSteps,yOffsetSteps;
#endif	
    int32_t zStart,zEnd;
#if !DISTORTION_PERMANENT
    int32_t matrix[DISTORTION_CORRECTION_POINTS * DISTORTION_CORRECTION_POINTS];
#endif
    bool enabled;
};
#endif //DISTORTION_CORRECTION

#define ENDSTOP_X_MIN_ID 1
#define ENDSTOP_X_MAX_ID 2
#define ENDSTOP_Y_MIN_ID 4
#define ENDSTOP_Y_MAX_ID 8
#define ENDSTOP_Z_MIN_ID 16
#define ENDSTOP_Z_MAX_ID 32
#define ENDSTOP_Z2_MIN_ID 64
#define ENDSTOP_Z_PROBE_ID 128

// These endstops are only used with EXTENDED_ENDSTOPS
#define ENDSTOP_X2_MIN_ID 1
#define ENDSTOP_X2_MAX_ID 2
#define ENDSTOP_Y2_MIN_ID 4
#define ENDSTOP_Y2_MAX_ID 8
#define ENDSTOP_Z2_MAX_ID 16
#define ENDSTOP_Z3_MIN_ID 32
#define ENDSTOP_Z3_MAX_ID 64

class Endstops {
    static flag8_t lastState;
    static flag8_t lastRead;
    static flag8_t accumulator;
#ifdef EXTENDED_ENDSTOPS
    static flag8_t lastState2;
    static flag8_t lastRead2;
    static flag8_t accumulator2;
#endif
public:
    static void update();
    static void report();
    static INLINE bool anyXYZMax() {
        return (lastState & (ENDSTOP_X_MAX_ID|ENDSTOP_Y_MAX_ID|ENDSTOP_Z_MAX_ID)) != 0;
    }
    static INLINE bool anyXYZ() {
#ifdef EXTENDED_ENDSTOPS
	    return (lastState & (ENDSTOP_X_MAX_ID|ENDSTOP_Y_MAX_ID|ENDSTOP_Z_MAX_ID|ENDSTOP_X_MIN_ID|ENDSTOP_Y_MIN_ID|ENDSTOP_Z_MIN_ID|ENDSTOP_Z2_MIN_ID)) != 0 ||
		lastState2 != 0;
#else
	    return (lastState & (ENDSTOP_X_MAX_ID|ENDSTOP_Y_MAX_ID|ENDSTOP_Z_MAX_ID|ENDSTOP_X_MIN_ID|ENDSTOP_Y_MIN_ID|ENDSTOP_Z_MIN_ID|ENDSTOP_Z2_MIN_ID)) != 0;
#endif
    }
    static INLINE void resetAccumulator() {
        accumulator = 0;
#ifdef EXTENDED_ENDSTOPS
        accumulator2 = 0;
#endif
    }
    static INLINE void fillFromAccumulator() {
        lastState = accumulator;
#ifdef EXTENDED_ENDSTOPS
        lastState2 = accumulator2;
#endif
    }
    static INLINE bool xMin() {
#if (X_MIN_PIN > -1) && MIN_HARDWARE_ENDSTOP_X
        return (lastState & ENDSTOP_X_MIN_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool xMax() {
#if (X_MAX_PIN > -1) && MAX_HARDWARE_ENDSTOP_X
        return (lastState & ENDSTOP_X_MAX_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool yMin() {
#if (Y_MIN_PIN > -1) && MIN_HARDWARE_ENDSTOP_Y
        return (lastState & ENDSTOP_Y_MIN_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool yMax() {
#if (Y_MAX_PIN > -1) && MAX_HARDWARE_ENDSTOP_Y
        return (lastState & ENDSTOP_Y_MAX_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool zMin() {
#if (Z_MIN_PIN > -1) && MIN_HARDWARE_ENDSTOP_Z
        return (lastState & ENDSTOP_Z_MIN_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool zMax() {
#if (Z_MAX_PIN > -1) && MAX_HARDWARE_ENDSTOP_Z
        return (lastState & ENDSTOP_Z_MAX_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool z2MinMax() {
#if (Z2_MINMAX_PIN > -1) && MINMAX_HARDWARE_ENDSTOP_Z2
        return (lastState & ENDSTOP_Z2_MINMAX_ID) != 0;
#else
        return false;
#endif
    }
    static INLINE bool zProbe() {
#if FEATURE_Z_PROBE
        return (lastState & ENDSTOP_Z_PROBE_ID) != 0;
#else
        return false;
#endif
    }
};

#ifndef DEFAULT_PRINTER_MODE
#if NUM_EXTRUDER > 0
#define DEFAULT_PRINTER_MODE PRINTER_MODE_FFF
#elif defined(SUPPORT_LASER) && SUPPORT_LASER
#define DEFAULT_PRINTER_MODE PRINTER_MODE_LASER
#elif defined(SUPPORT_CNC) && SUPPORT_CNC
#define DEFAULT_PRINTER_MODE PRINTER_MODE_CNC
#else
#error No supported printer mode compiled
#endif
#endif

class Printer
{
    static uint8_t debugLevel;
public:
#if USE_ADVANCE
    static volatile int extruderStepsNeeded; ///< This many extruder steps are still needed, <0 = reverse steps needed.
    static ufast8_t maxExtruderSpeed;            ///< Timer delay for end extruder speed
    //static uint8_t extruderAccelerateDelay;     ///< delay between 2 speec increases
    static int advanceStepsSet;
#if ENABLE_QUADRATIC_ADVANCE
    static long advanceExecuted;             ///< Executed advance steps
#endif
#endif
    static uint8_t menuMode;
    static float axisStepsPerMM[];
    static float invAxisStepsPerMM[];
    static float maxFeedrate[];
    static float homingFeedrate[];
    static float maxAccelerationMMPerSquareSecond[];
    static float maxTravelAccelerationMMPerSquareSecond[];
    static unsigned long maxPrintAccelerationStepsPerSquareSecond[];
    static unsigned long maxTravelAccelerationStepsPerSquareSecond[];
    static uint8_t relativeCoordinateMode;    ///< Determines absolute (false) or relative Coordinates (true).
    static uint8_t relativeExtruderCoordinateMode;  ///< Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.

    static uint8_t unitIsInches;
    static uint8_t mode;
    static uint8_t fanSpeed; // Last fan speed set with M106/M107
    static float zBedOffset;
    static uint8_t flag0,flag1; // 1 = stepper disabled, 2 = use external extruder interrupt, 4 = temp Sensor defect, 8 = homed
    static uint8_t flag2;
    static fast8_t stepsPerTimerCall;
    static uint32_t interval;    ///< Last step duration in ticks.
    static uint32_t timer;              ///< used for acceleration/deceleration timing
    static uint32_t stepNumber;         ///< Step number in current move.
    static float coordinateOffset[Z_AXIS_ARRAY];
    static int32_t currentPositionSteps[E_AXIS_ARRAY];     ///< Position in steps from origin.
    static float currentPosition[Z_AXIS_ARRAY];
    static float lastCmdPos[Z_AXIS_ARRAY]; ///< Last coordinates send by gcodes
    static int32_t destinationSteps[E_AXIS_ARRAY];         ///< Target position in steps.
    static float extrudeMultiplyError; ///< Accumulated error during extrusion
    static float extrusionFactor; ///< Extrusion multiply factor
#if NONLINEAR_SYSTEM
    static int32_t maxDeltaPositionSteps;
    static int32_t currentNonlinearPositionSteps[E_TOWER_ARRAY];
    static floatLong deltaDiagonalStepsSquaredA;
    static floatLong deltaDiagonalStepsSquaredB;
    static floatLong deltaDiagonalStepsSquaredC;
    static float deltaMaxRadiusSquared;
    static int32_t deltaFloorSafetyMarginSteps;
    static int32_t deltaAPosXSteps;
    static int32_t deltaAPosYSteps;
    static int32_t deltaBPosXSteps;
    static int32_t deltaBPosYSteps;
    static int32_t deltaCPosXSteps;
    static int32_t deltaCPosYSteps;
    static int32_t realDeltaPositionSteps[TOWER_ARRAY];
    static int16_t travelMovesPerSecond;
    static int16_t printMovesPerSecond;
    static float radius0;
#endif
#if DRIVE_SYSTEM != DELTA
	static int32_t zCorrectionStepsIncluded; 	
#endif
#if FEATURE_Z_PROBE || MAX_HARDWARE_ENDSTOP_Z || NONLINEAR_SYSTEM
    static int32_t stepsRemainingAtZHit;
#endif
#if DRIVE_SYSTEM == DELTA
    static int32_t stepsRemainingAtXHit;
    static int32_t stepsRemainingAtYHit;
#endif
#ifdef SOFTWARE_LEVELING
    static int32_t levelingP1[3];
    static int32_t levelingP2[3];
    static int32_t levelingP3[3];
#endif
#if FEATURE_AUTOLEVEL
    static float autolevelTransformation[9]; ///< Transformation matrix
#endif
#if FAN_THERMO_PIN > -1
	static float thermoMinTemp;
	static float thermoMaxTemp;
#endif
    static int16_t zBabystepsMissing;
    static float minimumSpeed;               ///< lowest allowed speed to keep integration error small
    static float minimumZSpeed;              ///< lowest allowed speed to keep integration error small
    static int32_t xMaxSteps;                   ///< For software endstops, limit of move in positive direction.
    static int32_t yMaxSteps;                   ///< For software endstops, limit of move in positive direction.
    static int32_t zMaxSteps;                   ///< For software endstops, limit of move in positive direction.
    static int32_t xMinSteps;                   ///< For software endstops, limit of move in negative direction.
    static int32_t yMinSteps;                   ///< For software endstops, limit of move in negative direction.
    static int32_t zMinSteps;                   ///< For software endstops, limit of move in negative direction.
    static float xLength;
    static float xMin;
    static float yLength;
    static float yMin;
    static float zLength;
    static float zMin;
    static float feedrate;                   ///< Last requested feedrate.
    static int feedrateMultiply;             ///< Multiplier for feedrate in percent (factor 1 = 100)
    static unsigned int extrudeMultiply;     ///< Flow multiplier in percdent (factor 1 = 100)
    static float maxJerk;                    ///< Maximum allowed jerk in mm/s
    static uint8_t interruptEvent;           ///< Event generated in interrupts that should/could be handled in main thread
#if DRIVE_SYSTEM!=DELTA
    static float maxZJerk;                   ///< Maximum allowed jerk in z direction in mm/s
#endif
    static float offsetX;                     ///< X-offset for different extruder positions.
    static float offsetY;                     ///< Y-offset for different extruder positions.
    static float offsetZ;                     ///< Y-offset for different extruder positions.
    static speed_t vMaxReached;         ///< Maximumu reached speed
    static uint32_t msecondsPrinting;            ///< Milliseconds of printing time (means time with heated extruder)
    static float filamentPrinted;            ///< mm of filament printed since counting started
#if ENABLE_BACKLASH_COMPENSATION
    static float backlashX;
    static float backlashY;
    static float backlashZ;
    static uint8_t backlashDir;
#endif
    static float memoryX;
    static float memoryY;
    static float memoryZ;
    static float memoryE;
    static float memoryF;
#if GANTRY && !defined(FAST_COREXYZ)
    static int8_t motorX;
    static int8_t motorYorZ;
#endif
#ifdef DEBUG_SEGMENT_LENGTH
    static float maxRealSegmentLength;
#endif
#ifdef DEBUG_REAL_JERK
    static float maxRealJerk;
#endif
    static fast8_t wizardStackPos;
    static wizardVar wizardStack[WIZARD_STACK_SIZE];

    static void handleInterruptEvent();

    static INLINE void setInterruptEvent(uint8_t evt, bool highPriority)
    {
        if(highPriority || interruptEvent == 0)
            interruptEvent = evt;
    }
    static void reportPrinterMode();
    static INLINE void setMenuMode(uint8_t mode,bool on)
    {
        if(on)
            menuMode |= mode;
        else
            menuMode &= ~mode;
    }

    static INLINE bool isMenuMode(uint8_t mode)
    {
        return (menuMode & mode) == mode;
    }
	static void setDebugLevel(uint8_t newLevel);
	static void toggleEcho();
	static void toggleInfo();
	static void toggleErrors();
	static void toggleDryRun();
	static void toggleCommunication();
	static void toggleNoMoves();
    static void toggleEndStop();
	static INLINE uint8_t getDebugLevel() {return debugLevel;}
    static INLINE bool debugEcho()
    {
        return ((debugLevel & 1) != 0);
    }

    static INLINE bool debugInfo()
    {
        return ((debugLevel & 2) != 0);
    }

    static INLINE bool debugErrors()
    {
        return ((debugLevel & 4) != 0);
    }

    static INLINE bool debugDryrun()
    {
        return ((debugLevel & 8) != 0);
    }

    static INLINE bool debugCommunication()
    {
        return ((debugLevel & 16) != 0);
    }

    static INLINE bool debugNoMoves()
    {
        return ((debugLevel & 32) != 0);
    }

    static INLINE bool debugEndStop()
    {
        return ((debugLevel & 64) != 0);
    }
    
    static INLINE bool debugFlag(uint8_t flags)
    {
        return (debugLevel & flags);
    }

    static INLINE void debugSet(uint8_t flags)
    {
        setDebugLevel(debugLevel | flags);
    }

    static INLINE void debugReset(uint8_t flags)
    {
        setDebugLevel(debugLevel & ~flags);
    }
    /** Sets the pwm for the fan speed. Gets called by motion control ot Commands::setFanSpeed. */
    static void setFanSpeedDirectly(uint8_t speed);
    static void setFan2SpeedDirectly(uint8_t speed);
    /** \brief Disable stepper motor for x direction. */
    static INLINE void disableXStepper()
    {
#if (X_ENABLE_PIN > -1)
        WRITE(X_ENABLE_PIN, !X_ENABLE_ON);
#endif
#if (FEATURE_TWO_XSTEPPER || DUAL_X_AXIS) && (X2_ENABLE_PIN > -1)
        WRITE(X2_ENABLE_PIN, !X_ENABLE_ON);
#endif
    }

    /** \brief Disable stepper motor for y direction. */
    static INLINE void disableYStepper()
    {
#if (Y_ENABLE_PIN > -1)
        WRITE(Y_ENABLE_PIN, !Y_ENABLE_ON);
#endif
#if FEATURE_TWO_YSTEPPER && (Y2_ENABLE_PIN > -1)
        WRITE(Y2_ENABLE_PIN, !Y_ENABLE_ON);
#endif
    }
    /** \brief Disable stepper motor for z direction. */
    static INLINE void disableZStepper()
    {
#if (Z_ENABLE_PIN > -1)
        WRITE(Z_ENABLE_PIN, !Z_ENABLE_ON);
#endif
#if FEATURE_TWO_ZSTEPPER && (Z2_ENABLE_PIN > -1)
        WRITE(Z2_ENABLE_PIN, !Z_ENABLE_ON);
#endif
#if FEATURE_THREE_ZSTEPPER && (Z3_ENABLE_PIN > -1)
        WRITE(Z3_ENABLE_PIN, !Z_ENABLE_ON);
#endif
    }

    /** \brief Enable stepper motor for x direction. */
    static INLINE void  enableXStepper()
    {
#if (X_ENABLE_PIN > -1)
        WRITE(X_ENABLE_PIN, X_ENABLE_ON);
#endif
#if (FEATURE_TWO_XSTEPPER || DUAL_X_AXIS) && (X2_ENABLE_PIN > -1)
        WRITE(X2_ENABLE_PIN, X_ENABLE_ON);
#endif
    }

    /** \brief Enable stepper motor for y direction. */
    static INLINE void  enableYStepper()
    {
#if (Y_ENABLE_PIN > -1)
        WRITE(Y_ENABLE_PIN, Y_ENABLE_ON);
#endif
#if FEATURE_TWO_YSTEPPER && (Y2_ENABLE_PIN > -1)
        WRITE(Y2_ENABLE_PIN, Y_ENABLE_ON);
#endif
    }
    /** \brief Enable stepper motor for z direction. */
    static INLINE void  enableZStepper()
    {
#if (Z_ENABLE_PIN > -1)
        WRITE(Z_ENABLE_PIN, Z_ENABLE_ON);
#endif
#if FEATURE_TWO_ZSTEPPER && (Z2_ENABLE_PIN > -1)
        WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON);
#endif
#if FEATURE_THREE_ZSTEPPER && (Z3_ENABLE_PIN > -1)
        WRITE(Z3_ENABLE_PIN, Z_ENABLE_ON);
#endif
    }

    static INLINE void setXDirection(bool positive)
    {
        if(positive)
        {
            WRITE(X_DIR_PIN,!INVERT_X_DIR);
#if FEATURE_TWO_XSTEPPER || DUAL_X_AXIS
            WRITE(X2_DIR_PIN,!INVERT_X_DIR);
#endif
        }
        else
        {
            WRITE(X_DIR_PIN,INVERT_X_DIR);
#if FEATURE_TWO_XSTEPPER || DUAL_X_AXIS
            WRITE(X2_DIR_PIN,INVERT_X_DIR);
#endif
        }
    }

    static INLINE void setYDirection(bool positive)
    {
        if(positive)
        {
            WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
#if FEATURE_TWO_YSTEPPER
            WRITE(Y2_DIR_PIN, !INVERT_Y_DIR);
#endif
        }
        else
        {
            WRITE(Y_DIR_PIN, INVERT_Y_DIR);
#if FEATURE_TWO_YSTEPPER
            WRITE(Y2_DIR_PIN, INVERT_Y_DIR);
#endif
        }
    }
    static INLINE void setZDirection(bool positive)
    {
        if(positive)
        {
            WRITE(Z_DIR_PIN, !INVERT_Z_DIR);
#if FEATURE_TWO_ZSTEPPER
            WRITE(Z2_DIR_PIN, !INVERT_Z_DIR);
#endif
#if FEATURE_THREE_ZSTEPPER
            WRITE(Z3_DIR_PIN, !INVERT_Z_DIR);
#endif
        }
        else
        {
            WRITE(Z_DIR_PIN, INVERT_Z_DIR);
#if FEATURE_TWO_ZSTEPPER
            WRITE(Z2_DIR_PIN, INVERT_Z_DIR);
#endif
#if FEATURE_THREE_ZSTEPPER
            WRITE(Z3_DIR_PIN, INVERT_Z_DIR);
#endif
        }
    }

    static INLINE bool getZDirection()
    {
        return ((READ(Z_DIR_PIN) != 0) ^ INVERT_Z_DIR);
    }

    static INLINE bool getYDirection()
    {
        return((READ(Y_DIR_PIN) != 0) ^ INVERT_Y_DIR);
    }

    static INLINE bool getXDirection()
    {
        return((READ(X_DIR_PIN) != 0) ^ INVERT_X_DIR);
    }

    static INLINE uint8_t isLargeMachine()
    {
        return flag0 & PRINTER_FLAG0_LARGE_MACHINE;
    }

    static INLINE void setLargeMachine(uint8_t b)
    {
        flag0 = (b ? flag0 | PRINTER_FLAG0_LARGE_MACHINE : flag0 & ~PRINTER_FLAG0_LARGE_MACHINE);
    }

    static INLINE uint8_t isAdvanceActivated()
    {
        return flag0 & PRINTER_FLAG0_SEPERATE_EXTRUDER_INT;
    }

    static INLINE void setAdvanceActivated(uint8_t b)
    {
        flag0 = (b ? flag0 | PRINTER_FLAG0_SEPERATE_EXTRUDER_INT : flag0 & ~PRINTER_FLAG0_SEPERATE_EXTRUDER_INT);
    }

    static INLINE uint8_t isHomed()
    {
        return flag1 & PRINTER_FLAG1_HOMED;
    }

    static INLINE void setHomed(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_HOMED : flag1 & ~PRINTER_FLAG1_HOMED);
    }

    static INLINE uint8_t isAllKilled()
    {
        return flag1 & PRINTER_FLAG1_ALLKILLED;
    }

    static INLINE void setAllKilled(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_ALLKILLED : flag1 & ~PRINTER_FLAG1_ALLKILLED);
    }

    static INLINE uint8_t isAutomount()
    {
        return flag1 & PRINTER_FLAG1_AUTOMOUNT;
    }

    static INLINE void setAutomount(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_AUTOMOUNT : flag1 & ~PRINTER_FLAG1_AUTOMOUNT);
    }

    static INLINE uint8_t isAnimation()
    {
        return flag1 & PRINTER_FLAG1_ANIMATION;
    }

    static INLINE void setAnimation(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_ANIMATION : flag1 & ~PRINTER_FLAG1_ANIMATION);
    }

    static INLINE uint8_t isUIErrorMessage()
    {
        return flag1 & PRINTER_FLAG1_UI_ERROR_MESSAGE;
    }

    static INLINE void setUIErrorMessage(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_UI_ERROR_MESSAGE : flag1 & ~PRINTER_FLAG1_UI_ERROR_MESSAGE);
    }

    static INLINE uint8_t isNoDestinationCheck()
    {
        return flag1 & PRINTER_FLAG1_NO_DESTINATION_CHECK;
    }

    static INLINE void setNoDestinationCheck(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_NO_DESTINATION_CHECK : flag1 & ~PRINTER_FLAG1_NO_DESTINATION_CHECK);
    }

    static INLINE uint8_t isPowerOn()
    {
        return flag1 & PRINTER_FLAG1_POWER_ON;
    }

    static INLINE void setPowerOn(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_POWER_ON : flag1 & ~PRINTER_FLAG1_POWER_ON);
    }

    static INLINE uint8_t isColdExtrusionAllowed()
    {
        return flag1 & PRINTER_FLAG1_ALLOW_COLD_EXTRUSION;
    }

    static INLINE void setColdExtrusionAllowed(uint8_t b)
    {
        flag1 = (b ? flag1 | PRINTER_FLAG1_ALLOW_COLD_EXTRUSION : flag1 & ~PRINTER_FLAG1_ALLOW_COLD_EXTRUSION);
        if(b)
            Com::printFLN(PSTR("Cold extrusion allowed"));
        else
            Com::printFLN(PSTR("Cold extrusion disallowed"));
    }

    static INLINE uint8_t isBlockingReceive()
    {
        return flag2 & PRINTER_FLAG2_BLOCK_RECEIVING;
    }

    static INLINE void setBlockingReceive(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_BLOCK_RECEIVING : flag2 & ~PRINTER_FLAG2_BLOCK_RECEIVING);
        Com::printFLN(b ? Com::tPauseCommunication : Com::tContinueCommunication);
    }

    static INLINE uint8_t isAutoretract()
    {
        return flag2 & PRINTER_FLAG2_AUTORETRACT;
    }

    static INLINE void setAutoretract(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_AUTORETRACT : flag2 & ~PRINTER_FLAG2_AUTORETRACT);
        Com::printFLN(PSTR("Autoretract:"),b);
    }
    static INLINE uint8_t isHoming()
    {
        return flag2 & PRINTER_FLAG2_HOMING;
    }

    static INLINE void setHoming(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_HOMING : flag2 & ~PRINTER_FLAG2_HOMING);
    }
    static INLINE uint8_t isAllEMotors()
    {
        return flag2 & PRINTER_FLAG2_ALL_E_MOTORS;
    }

    static INLINE void setAllEMotors(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_ALL_E_MOTORS : flag2 & ~PRINTER_FLAG2_ALL_E_MOTORS);
    }

    static INLINE uint8_t isDebugJam()
    {
        return (flag2 & PRINTER_FLAG2_DEBUG_JAM) != 0;
    }

    static INLINE uint8_t isDebugJamOrDisabled()
    {
        return (flag2 & (PRINTER_FLAG2_DEBUG_JAM | PRINTER_FLAG2_JAMCONTROL_DISABLED)) != 0;
    }

    static INLINE void setDebugJam(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_DEBUG_JAM : flag2 & ~PRINTER_FLAG2_DEBUG_JAM);
        Com::printFLN(PSTR("Jam debugging:"),b);
    }

    static INLINE uint8_t isJamcontrolDisabled()
    {
        return (flag2 & PRINTER_FLAG2_JAMCONTROL_DISABLED) != 0;
    }

    static INLINE void setJamcontrolDisabled(uint8_t b)
    {
        flag2 = (b ? flag2 | PRINTER_FLAG2_JAMCONTROL_DISABLED : flag2 & ~PRINTER_FLAG2_JAMCONTROL_DISABLED);
        Com::printFLN(PSTR("Jam control disabled:"),b);
    }

    static INLINE void toggleAnimation()
    {
        setAnimation(!isAnimation());
    }
    static INLINE float convertToMM(float x)
    {
        return (unitIsInches ? x*25.4 : x);
    }
    static INLINE bool areAllSteppersDisabled()
    {
        return flag0 & PRINTER_FLAG0_STEPPER_DISABLED;
    }
    static INLINE void setAllSteppersDiabled()
    {
        flag0 |= PRINTER_FLAG0_STEPPER_DISABLED;
    }
    static INLINE void unsetAllSteppersDisabled()
    {
        flag0 &= ~PRINTER_FLAG0_STEPPER_DISABLED;
#if FAN_BOARD_PIN > -1
        pwm_pos[PWM_BOARD_FAN] = BOARD_FAN_SPEED;
#endif // FAN_BOARD_PIN
    }
    static INLINE bool isAnyTempsensorDefect()
    {
        return (flag0 & PRINTER_FLAG0_TEMPSENSOR_DEFECT);
    }
    static INLINE void setAnyTempsensorDefect()
    {
        flag0 |= PRINTER_FLAG0_TEMPSENSOR_DEFECT;
		debugSet(8);
    }
    static INLINE void unsetAnyTempsensorDefect()
    {
        flag0 &= ~PRINTER_FLAG0_TEMPSENSOR_DEFECT;
    }
    static INLINE bool isManualMoveMode()
    {
        return (flag0 & PRINTER_FLAG0_MANUAL_MOVE_MODE);
    }
    static INLINE void setManualMoveMode(bool on)
    {
        flag0 = (on ? flag0 | PRINTER_FLAG0_MANUAL_MOVE_MODE : flag0 & ~PRINTER_FLAG0_MANUAL_MOVE_MODE);
    }
    static INLINE bool isAutolevelActive()
    {
        return (flag0 & PRINTER_FLAG0_AUTOLEVEL_ACTIVE)!=0;
    }
    static void setAutolevelActive(bool on);
    static INLINE void setZProbingActive(bool on)
    {
        flag0 = (on ? flag0 | PRINTER_FLAG0_ZPROBEING : flag0 & ~PRINTER_FLAG0_ZPROBEING);
    }
    static INLINE bool isZProbingActive()
    {
        return (flag0 & PRINTER_FLAG0_ZPROBEING);
    }
    static INLINE void executeXYGantrySteps()
    {
#if (GANTRY) && !defined(FAST_COREXYZ)
        if(motorX <= -2)
        {
            WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER
            WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorX += 2;
        }
        else if(motorX >= 2)
        {
            WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER
            WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorX -= 2;
        }
        if(motorYorZ <= -2)
        {
            WRITE(Y_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_YSTEPPER
            WRITE(Y2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorYorZ += 2;
        }
        else if(motorYorZ >= 2)
        {
            WRITE(Y_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_YSTEPPER
            WRITE(Y2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorYorZ -= 2;
        }
#endif
    }
    static INLINE void executeXZGantrySteps()
    {
#if (GANTRY) && !defined(FAST_COREXYZ)
        if(motorX <= -2)
        {
            WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER
            WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorX += 2;
        }
        else if(motorX >= 2)
        {
            WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER
            WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorX -= 2;
        }
        if(motorYorZ <= -2)
        {
            WRITE(Z_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_ZSTEPPER
            WRITE(Z2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
#if FEATURE_THREE_ZSTEPPER
            WRITE(Z3_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorYorZ += 2;
        }
        else if(motorYorZ >= 2)
        {
            WRITE(Z_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_ZSTEPPER
            WRITE(Z2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
#if FEATURE_THREE_ZSTEPPER
            WRITE(Z3_STEP_PIN,START_STEP_WITH_HIGH);
#endif
            motorYorZ -= 2;
        }
#endif
    }
    static INLINE void startXStep()
    {
#if DUAL_X_AXIS
#if FEATURE_DITTO_PRINTING
		if(Extruder::dittoMode) {
			WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
			WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
			return;
		}
#endif
		if(Extruder::current->id) {
			WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);			
		} else {
			WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);			
		}
#else		
        WRITE(X_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER
        WRITE(X2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
#endif
    }
    static INLINE void startYStep()
    {
        WRITE(Y_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_YSTEPPER
        WRITE(Y2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
    }
    static INLINE void startZStep()
    {
        WRITE(Z_STEP_PIN,START_STEP_WITH_HIGH);
#if FEATURE_TWO_ZSTEPPER
        WRITE(Z2_STEP_PIN,START_STEP_WITH_HIGH);
#endif
#if FEATURE_THREE_ZSTEPPER
        WRITE(Z3_STEP_PIN,START_STEP_WITH_HIGH);
#endif
    }
    static INLINE void endXYZSteps()
    {
        WRITE(X_STEP_PIN,!START_STEP_WITH_HIGH);
#if FEATURE_TWO_XSTEPPER || DUAL_X_AXIS
        WRITE(X2_STEP_PIN,!START_STEP_WITH_HIGH);
#endif
        WRITE(Y_STEP_PIN,!START_STEP_WITH_HIGH);
#if FEATURE_TWO_YSTEPPER
        WRITE(Y2_STEP_PIN,!START_STEP_WITH_HIGH);
#endif
        WRITE(Z_STEP_PIN,!START_STEP_WITH_HIGH);
#if FEATURE_TWO_ZSTEPPER
        WRITE(Z2_STEP_PIN,!START_STEP_WITH_HIGH);
#endif
#if FEATURE_THREE_ZSTEPPER
        WRITE(Z3_STEP_PIN,!START_STEP_WITH_HIGH);
#endif
    }
    static INLINE speed_t updateStepsPerTimerCall(speed_t vbase)
    {
        if(vbase > STEP_DOUBLER_FREQUENCY)
        {
#if ALLOW_QUADSTEPPING
            if(vbase > STEP_DOUBLER_FREQUENCY * 2)
            {
                Printer::stepsPerTimerCall = 4;
                return vbase >> 2;
            }
            else
            {
                Printer::stepsPerTimerCall = 2;
                return vbase >> 1;
            }
#else
            Printer::stepsPerTimerCall = 2;
            return vbase >> 1;
#endif
        }
        else
        {
            Printer::stepsPerTimerCall = 1;
        }
        return vbase;
    }
    static INLINE void disableAllowedStepper()
    {
#if DRIVE_SYSTEM == XZ_GANTRY || DRIVE_SYSTEM == ZX_GANTRY
        if(DISABLE_X && DISABLE_Z)
        {
            disableXStepper();
            disableZStepper();
        }
        if(DISABLE_Y) disableYStepper();
#else
#if GANTRY
        if(DISABLE_X && DISABLE_Y)
        {
            disableXStepper();
            disableYStepper();
        }
#else
        if(DISABLE_X) disableXStepper();
        if(DISABLE_Y) disableYStepper();
#endif
        if(DISABLE_Z) disableZStepper();
#endif
    }
    static INLINE float realXPosition()
    {
        return currentPosition[X_AXIS];
    }

    static INLINE float realYPosition()
    {
        return currentPosition[Y_AXIS];
    }

    static INLINE float realZPosition()
    {
        return currentPosition[Z_AXIS];
    }
    static INLINE void realPosition(float &xp, float &yp, float &zp)
    {
        xp = currentPosition[X_AXIS];
        yp = currentPosition[Y_AXIS];
        zp = currentPosition[Z_AXIS];
    }
    static INLINE void insertStepperHighDelay()
    {
#if STEPPER_HIGH_DELAY>0
        HAL::delayMicroseconds(STEPPER_HIGH_DELAY);
#endif
    }
    static void constrainDestinationCoords();
    static void updateDerivedParameter();
    static void updateCurrentPosition(bool copyLastCmd = false);
    static void kill(uint8_t only_steppers);
    static void updateAdvanceFlags();
    static void setup();
    static void defaultLoopActions();
    static uint8_t setDestinationStepsFromGCode(GCode *com);
    static uint8_t moveTo(float x,float y,float z,float e,float f);
    static uint8_t moveToReal(float x,float y,float z,float e,float f,bool pathOptimize = true);
    static void homeAxis(bool xaxis,bool yaxis,bool zaxis); /// Home axis
    static void setOrigin(float xOff,float yOff,float zOff);
    static bool isPositionAllowed(float x,float y,float z);
    static INLINE int getFanSpeed()
    {
        return (int)pwm_pos[PWM_FAN1];
    }
    static INLINE int getFan2Speed()
    {
	    return (int)pwm_pos[PWM_FAN2];
    }
#if NONLINEAR_SYSTEM
    static INLINE void setDeltaPositions(long xaxis, long yaxis, long zaxis)
    {
        currentNonlinearPositionSteps[A_TOWER] = xaxis;
        currentNonlinearPositionSteps[B_TOWER] = yaxis;
        currentNonlinearPositionSteps[C_TOWER] = zaxis;
    }
    static void deltaMoveToTopEndstops(float feedrate);
#endif
#if MAX_HARDWARE_ENDSTOP_Z
    static float runZMaxProbe();
#endif
#if FEATURE_Z_PROBE
	static void startProbing(bool runScript);
	static void finishProbing();
    static float runZProbe(bool first,bool last,uint8_t repeat = Z_PROBE_REPETITIONS,bool runStartScript = true);
    static void waitForZProbeStart();
    static float bendingCorrectionAt(float x,float y);
#endif
    // Moved outside FEATURE_Z_PROBE to allow auto-level functional test on
    // system without Z-probe
    static void transformToPrinter(float x,float y,float z,float &transX,float &transY,float &transZ);
    static void transformFromPrinter(float x,float y,float z,float &transX,float &transY,float &transZ);
#if FEATURE_AUTOLEVEL
    static void resetTransformationMatrix(bool silent);
    //static void buildTransformationMatrix(float h1,float h2,float h3);
    static void buildTransformationMatrix(Plane &plane);
#endif
#if DISTORTION_CORRECTION
    static bool measureDistortion(void);
    static Distortion distortion;
#endif
    static void MemoryPosition();
    static void GoToMemoryPosition(bool x,bool y,bool z,bool e,float feed);
    static void zBabystep();

    static INLINE void resetWizardStack()
    {
        wizardStackPos = 0;
    }
    static INLINE void pushWizardVar(wizardVar v)
    {
        wizardStack[wizardStackPos++] = v;
    }
    static INLINE wizardVar popWizardVar()
    {
        return wizardStack[--wizardStackPos];
    }
    static void showConfiguration();
    static void setCaseLight(bool on);
    static void reportCaseLightStatus();
#if JSON_OUTPUT
    static void showJSONStatus(int type);
#endif
    static void homeXAxis();
    static void homeYAxis();
    static void homeZAxis();
};

#endif // PRINTER_H_INCLUDED