/* -*- c++ -*- */

/*
    Reprap firmware based on Sprinter and grbl.
 Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm

 This program 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.

 This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 This firmware is a mashup between Sprinter and grbl.
  (https://github.com/kliment/Sprinter)
  (https://github.com/simen/grbl/tree)

 It has preliminary support for Matthew Roberts advance algorithm
    http://reprap.org/pipermail/reprap-dev/2011-May/003323.html
 */

#include "Marlin.h"

#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
  #ifdef AUTO_BED_LEVELING_GRID
    #include "qr_solve.h"
  #endif
#endif // ENABLE_AUTO_BED_LEVELING

#include "ultralcd.h"
#include "planner.h"
#include "stepper.h"
#include "temperature.h"
#include "motion_control.h"
#include "cardreader.h"
#include "watchdog.h"
#include "ConfigurationStore.h"
#include "language.h"
#include "pins_arduino.h"
#include "math.h"

#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
#include <SPI.h>
#endif

#define VERSION_STRING  "1.0.0"

// look here for descriptions of G-codes: http://linuxcnc.org/handbook/gcode/g-code.html
// http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes

//Implemented Codes
//-------------------
// G0  -> G1
// G1  - Coordinated Movement X Y Z E
// G2  - CW ARC
// G3  - CCW ARC
// G4  - Dwell S<seconds> or P<milliseconds>
// G10 - retract filament according to settings of M207
// G11 - retract recover filament according to settings of M208
// G28 - Home all Axis
// G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
// G30 - Single Z Probe, probes bed at current XY location.
// G90 - Use Absolute Coordinates
// G91 - Use Relative Coordinates
// G92 - Set current position to coordinates given

// M Codes
// M0   - Unconditional stop - Wait for user to press a button on the LCD (Only if ULTRA_LCD is enabled)
// M1   - Same as M0
// M17  - Enable/Power all stepper motors
// M18  - Disable all stepper motors; same as M84
// M20  - List SD card
// M21  - Init SD card
// M22  - Release SD card
// M23  - Select SD file (M23 filename.g)
// M24  - Start/resume SD print
// M25  - Pause SD print
// M26  - Set SD position in bytes (M26 S12345)
// M27  - Report SD print status
// M28  - Start SD write (M28 filename.g)
// M29  - Stop SD write
// M30  - Delete file from SD (M30 filename.g)
// M31  - Output time since last M109 or SD card start to serial
// M32  - Select file and start SD print (Can be used _while_ printing from SD card files):
//        syntax "M32 /path/filename#", or "M32 S<startpos bytes> !filename#"
//        Call gcode file : "M32 P !filename#" and return to caller file after finishing (similar to #include).
//        The '#' is necessary when calling from within sd files, as it stops buffer prereading
// M42  - Change pin status via gcode Use M42 Px Sy to set pin x to value y, when omitting Px the onboard led will be used.
// M80  - Turn on Power Supply
// M81  - Turn off Power Supply
// M82  - Set E codes absolute (default)
// M83  - Set E codes relative while in Absolute Coordinates (G90) mode
// M84  - Disable steppers until next move,
//        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
// M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
// M92  - Set axis_steps_per_unit - same syntax as G92
// M104 - Set extruder target temp
// M105 - Read current temp
// M106 - Fan on
// M107 - Fan off
// M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
//        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
// M114 - Output current position to serial port
// M115 - Capabilities string
// M117 - display message
// M119 - Output Endstop status to serial port
// M140 - Set bed target temp
// M150 - Set BlinkM Color Output R: Red<0-255> U(!): Green<0-255> B: Blue<0-255> over i2c, G for green does not work.
// M190 - Sxxx Wait for bed current temp to reach target temp. Waits only when heating
//        Rxxx Wait for bed current temp to reach target temp. Waits when heating and cooling
// M200 D<millimeters>- set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
// M201 - Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
// M202 - Set max acceleration in units/s^2 for travel moves (M202 X1000 Y1000) Unused in Marlin!!
// M203 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) in mm/sec^2  also sets minimum segment time in ms (B20000) to prevent buffer under-runs and M20 minimum feedrate
// M205 -  advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk, E=maximum E jerk
// M206 - set additional homing offset
// M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop], stays in mm regardless of M200 setting
// M208 - set recover=unretract length S[positive mm surplus to the M207 S*] F[feedrate mm/sec]
// M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
// M218 - set hotend offset (in mm): T<extruder_number> X<offset_on_X> Y<offset_on_Y>
// M220 S<factor in percent>- set speed factor override percentage
// M221 S<factor in percent>- set extrude factor override percentage
// M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
// M250 - Set LCD contrast C<contrast value> (value 0..63)
// M280 - set servo position absolute. P: servo index, S: angle or microseconds
// M300 - Play beep sound S<frequency Hz> P<duration ms>
// M301 - Set PID parameters P I and D
// M302 - Allow cold extrudes, or set the minimum extrude S<temperature>.
// M303 - PID relay autotune S<temperature> sets the target temperature. (default target temperature = 150C)
// M304 - Set bed PID parameters P I and D
// M400 - Finish all moves
// M401 - Lower z-probe if present
// M402 - Raise z-probe if present
// M500 - stores parameters in EEPROM
// M501 - reads parameters from EEPROM (if you need reset them after you changed them temporarily).
// M502 - reverts to the default "factory settings".  You still need to store them in EEPROM afterwards if you want to.
// M503 - print the current settings (from memory not from EEPROM)
// M540 - Use S[0|1] to enable or disable the stop SD card print on endstop hit (requires ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
// M600 - Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
// M665 - set delta configurations
// M666 - set delta endstop adjustment
// M605 - Set dual x-carriage movement mode: S<mode> [ X<duplication x-offset> R<duplication temp offset> ]
// M700 - Level plate script for use with Witbox printer.
// M701 - Load filament script for use with Witbox printer.
// M702 - Unload filament script for use with Witbox printer.
// M907 - Set digital trimpot motor current using axis codes.
// M908 - Control digital trimpot directly.
// M350 - Set microstepping mode.
// M351 - Toggle MS1 MS2 pins directly.
// M928 - Start SD logging (M928 filename.g) - ended by M29
// M999 - Restart after being stopped by error

//Stepper Movement Variables

//===========================================================================
//=============================imported variables============================
//===========================================================================


//===========================================================================
//=============================public variables=============================
//===========================================================================
#ifdef SDSUPPORT
CardReader card;
#endif
float homing_feedrate[] = HOMING_FEEDRATE;
bool axis_relative_modes[] = AXIS_RELATIVE_MODES;
int feedmultiply=100; //100->1 200->2
int saved_feedmultiply;
int extrudemultiply=100; //100->1 200->2
float volumetric_multiplier[EXTRUDERS] = {1.0
  #if EXTRUDERS > 1
    , 1.0
    #if EXTRUDERS > 2
      , 1.0
    #endif
  #endif
};
float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
float add_homeing[3]={0,0,0};
float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
bool axis_known_position[3] = {false, false, false};
float zprobe_zoffset;

// Extruder offset
#if EXTRUDERS > 1
#ifndef DUAL_X_CARRIAGE
  #define NUM_EXTRUDER_OFFSETS 2 // only in XY plane
#else
  #define NUM_EXTRUDER_OFFSETS 3 // supports offsets in XYZ plane
#endif
float extruder_offset[NUM_EXTRUDER_OFFSETS][EXTRUDERS] = {
#if defined(EXTRUDER_OFFSET_X) && defined(EXTRUDER_OFFSET_Y)
  EXTRUDER_OFFSET_X, EXTRUDER_OFFSET_Y
#endif
};
#endif
uint8_t active_extruder = 0;
int fanSpeed=0;

bool stop_buffer = false;
int stop_buffer_code = 0;

uint8_t buffer_recursivity = 0;

#ifdef ULTIPANEL
  #ifdef PS_DEFAULT_OFF
    bool powersupply = false;
  #else
	bool powersupply = true;
  #endif
#endif				

bool cancel_heatup = false;

//===========================================================================
//=============================Private Variables=============================
//===========================================================================
const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
static float offset[3] = {0.0, 0.0, 0.0};
static bool home_all_axis = true;
static float feedrate = 1500.0, next_feedrate, saved_feedrate;
static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;

static bool relative_mode = false;  //Determines Absolute or Relative Coordinates

static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
static bool fromsd[BUFSIZE];
static int bufindr = 0;
static int bufindw = 0;
static int buflen = 0;
static char serial_char;
static int serial_count = 0;
static boolean comment_mode = false;
static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc

const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42

//Inactivity shutdown variables
static unsigned long previous_millis_cmd = 0;
static unsigned long max_inactive_time = 0;
static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;

unsigned long starttime=0;
unsigned long stoptime=0;

static uint8_t tmp_extruder;

bool Stopped=false;

bool CooldownNoWait = true;
bool target_direction;


//===========================================================================
//=============================Routines======================================
//===========================================================================

void get_arc_coordinates();
bool setTargetedHotend(int code);

void serial_echopair_P(const char *s_P, float v)
    { serialprintPGM(s_P); SERIAL_ECHO(v); }
void serial_echopair_P(const char *s_P, double v)
    { serialprintPGM(s_P); SERIAL_ECHO(v); }
void serial_echopair_P(const char *s_P, unsigned long v)
    { serialprintPGM(s_P); SERIAL_ECHO(v); }

extern "C"{
  extern unsigned int __bss_end;
  extern unsigned int __heap_start;
  extern void *__brkval;

  int freeMemory() {
    int free_memory;

    if((int)__brkval == 0)
      free_memory = ((int)&free_memory) - ((int)&__bss_end);
    else
      free_memory = ((int)&free_memory) - ((int)__brkval);

    return free_memory;
  }
}

//adds an command to the main command buffer
//thats really done in a non-safe way.
//needs overworking someday
void enquecommand(const char *cmd)
{
	if(buflen < BUFSIZE)
	{
		//this is dangerous if a mixing of serial and this happens
		strcpy(&(cmdbuffer[bufindw][0]),cmd);
		SERIAL_ECHO_START;
		SERIAL_ECHOPGM("enqueing \"");
		SERIAL_ECHO(cmdbuffer[bufindw]);
		SERIAL_ECHOLNPGM("\"");
		bufindw= (bufindw + 1)%BUFSIZE;
		buflen += 1;
	}
}

void enquecommand_P(const char *cmd)
{
	if(buflen < BUFSIZE)
	{
		//this is dangerous if a mixing of serial and this happens
		strcpy_P(&(cmdbuffer[bufindw][0]),cmd);
		SERIAL_ECHO_START;
		SERIAL_ECHOPGM("enqueing \"");
		SERIAL_ECHO(cmdbuffer[bufindw]);
		SERIAL_ECHOLNPGM("\"");
		bufindw= (bufindw + 1)%BUFSIZE;
		buflen += 1;
	}
}

// Discard all gcodes enqueued in the gcode-buffer
void flush_commands()
{
	bufindr = bufindw;
	buflen = 0;
}

void setup_killpin()
{
	#if defined(KILL_PIN) && KILL_PIN > -1
		pinMode(KILL_PIN,INPUT);
		WRITE(KILL_PIN,HIGH);
	#endif
}

void setup_powerhold()
{
	#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
		SET_OUTPUT(SUICIDE_PIN);
		WRITE(SUICIDE_PIN, HIGH);
	#endif
	#if defined(PS_ON_PIN) && PS_ON_PIN > -1
		SET_OUTPUT(PS_ON_PIN);
		#if defined(PS_DEFAULT_OFF)
			WRITE(PS_ON_PIN, PS_ON_ASLEEP);
		#else
			WRITE(PS_ON_PIN, PS_ON_AWAKE);
		#endif
	#endif
}

void suicide()
{
	#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
		SET_OUTPUT(SUICIDE_PIN);
		WRITE(SUICIDE_PIN, LOW);
	#endif
}

void setup()
{
	setup_killpin();
	setup_powerhold();
	MYSERIAL.begin(BAUDRATE);
	SERIAL_PROTOCOLLNPGM("start");
	SERIAL_ECHO_START;

	// Check startup - does nothing if bootloader sets MCUSR to 0
	byte mcu = MCUSR;
	if(mcu & 1) SERIAL_ECHOLNPGM(MSG_POWERUP);
	if(mcu & 2) SERIAL_ECHOLNPGM(MSG_EXTERNAL_RESET);
	if(mcu & 4) SERIAL_ECHOLNPGM(MSG_BROWNOUT_RESET);
	if(mcu & 8) SERIAL_ECHOLNPGM(MSG_WATCHDOG_RESET);
	if(mcu & 32) SERIAL_ECHOLNPGM(MSG_SOFTWARE_RESET);
	MCUSR=0;

	SERIAL_ECHOPGM(MSG_MARLIN);
	SERIAL_ECHOLNPGM(VERSION_STRING);
	#ifdef STRING_VERSION_CONFIG_H
		#ifdef STRING_CONFIG_H_AUTHOR
			SERIAL_ECHO_START;
			SERIAL_ECHOPGM(MSG_CONFIGURATION_VER);
			SERIAL_ECHOPGM(STRING_VERSION_CONFIG_H);
			SERIAL_ECHOPGM(MSG_AUTHOR);
			SERIAL_ECHOLNPGM(STRING_CONFIG_H_AUTHOR);
			SERIAL_ECHOPGM("Compiled: ");
			SERIAL_ECHOLNPGM(__DATE__);
		#endif
	#endif
	SERIAL_ECHO_START;
	SERIAL_ECHOPGM(MSG_FREE_MEMORY);
	SERIAL_ECHO(freeMemory());
	SERIAL_ECHOPGM(MSG_PLANNER_BUFFER_BYTES);
	SERIAL_ECHOLN((int)sizeof(block_t)*BLOCK_BUFFER_SIZE);
	for(int8_t i = 0; i < BUFSIZE; i++)
	{
		fromsd[i] = false;
	}

	// loads data from EEPROM if available else uses defaults (and resets step acceleration rate)
	Config_RetrieveSettings();

	tp_init();    // Initialize temperature loop
	plan_init();  // Initialize planner;
	watchdog_init();
  
	lcd_init();

	st_init();    // Initialize stepper, this enables interrupts!

	_delay_ms(1000);	// wait 1sec to display the splash screen

	#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
		SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
	#endif

	#ifdef DIGIPOT_I2C
		digipot_i2c_init();
	#endif
}

char *test;

void loop()
{
	if (stop_buffer == false) {
    	if (buflen < (BUFSIZE-1)) {
			get_command();
		}
	} else {
      switch (stop_buffer_code) {
      case 1:
        enquecommand_P(PSTR("M25"));
		stop_buffer_code = 0;
        break;
      case 2:
        enquecommand_P(PSTR("M600"));
		stop_buffer_code = 0;
        break;
      case 999:
        get_command();
        bufindr = (bufindw - 1);
        if (bufindr < 0) { bufindr = BUFSIZE - 1; };
		test = cmdbuffer[bufindr];
        if (strstr(test, "M999") == NULL) {
          bufindr = bufindw;
          buflen = 0;
        } else {
          stop_buffer = false;
          stop_buffer_code = 0;
          bufindr = 0;
          bufindw = 0;
          buflen = 0;
          FlushSerialRequestResend();
          lcd_reset_alert_level();
          LCD_MESSAGEPGM(WELCOME_MSG);
        }
        break;
      default:
        break;
      }
    }
	
	#ifdef SDSUPPORT
		card.checkautostart(false);
	#endif // SDSUPPORT

	if(buflen) {
		#ifdef SDSUPPORT
			if(card.saving) {
				if(strstr_P(cmdbuffer[bufindr], PSTR("M29")) == NULL) {
					card.write_command(cmdbuffer[bufindr]);
					if(card.logging) {
						process_commands();
					} else {
						SERIAL_PROTOCOLLNPGM(MSG_OK);
					}
      			} else {
					card.closefile();
					SERIAL_PROTOCOLLNPGM(MSG_FILE_SAVED);
      			}
			} else {
				process_commands();
			}		
		#else
			process_commands();
		#endif //SDSUPPORT

		buflen = (buflen-1);
		bufindr = (bufindr + 1) % BUFSIZE;
	}
	
	//check heater every n milliseconds
	manage_heater();
	manage_inactivity();
	checkHitEndstops();
	lcd_update();
}

void get_command() 
{
	while(MYSERIAL.available() > 0  && buflen < BUFSIZE) {
		serial_char = MYSERIAL.read();
		if(serial_char == '\n' || 
		  serial_char == '\r' ||
		  (serial_char == ':' && comment_mode == false) ||
		  serial_count >= (MAX_CMD_SIZE - 1) ) {

			if(!serial_count) { //if empty line
				comment_mode = false; //for new command
				return;
			}
			
			cmdbuffer[bufindw][serial_count] = 0; //terminate string
			if(!comment_mode) {
				comment_mode = false; //for new command
				fromsd[bufindw] = false;
				if(strchr(cmdbuffer[bufindw], 'N') != NULL) {
					strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
					gcode_N = (strtol(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL, 10));
					if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
						SERIAL_ERRORLN(gcode_LastN);
						//Serial.println(gcode_N);
						FlushSerialRequestResend();
						serial_count = 0;
						return;
					}

					if(strchr(cmdbuffer[bufindw], '*') != NULL) {
						byte checksum = 0;
						byte count = 0;
						while(cmdbuffer[bufindw][count] != '*') 
							checksum = checksum^cmdbuffer[bufindw][count++];
						strchr_pointer = strchr(cmdbuffer[bufindw], '*');

						if( (int)(strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)) != checksum) {
							SERIAL_ERROR_START;
							SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
							SERIAL_ERRORLN(gcode_LastN);
							FlushSerialRequestResend();
							serial_count = 0;
							return;
						}
	    				//if no errors, continue parsing
					} else {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
						SERIAL_ERRORLN(gcode_LastN);
						FlushSerialRequestResend();
						serial_count = 0;
						return;
					}

					gcode_LastN = gcode_N;
					//if no errors, continue parsing
				} else { // if we don't receive 'N' but still see '*'
					if((strchr(cmdbuffer[bufindw], '*') != NULL)) {
						SERIAL_ERROR_START;
						SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
						SERIAL_ERRORLN(gcode_LastN);
						serial_count = 0;
						return;
					}
				}

				if((strchr(cmdbuffer[bufindw], 'G') != NULL)) {
					strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
					switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
						case 0:
						case 1:
						case 2:
						case 3:
							if (Stopped == true) {
								SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
								LCD_MESSAGEPGM(MSG_STOPPED);
                				lcd_update();
							}
		    				break;
						default:
							break;
					}
				}
				bufindw = (bufindw + 1)%BUFSIZE;
				buflen += 1;
			}
			serial_count = 0; //clear buffer
		} else {
			if(serial_char == ';') 
				comment_mode = true;
			if(!comment_mode) 
				cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}

	#ifdef SDSUPPORT
	if(!card.sdprinting || serial_count!=0)
		return;

	//'#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
	// if it occurs, stop_buffering is triggered and the buffer is ran dry.
	// this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing

	static bool stop_buffering=false;
	if(buflen==0) stop_buffering=false;

	while( !card.eof()  && buflen < BUFSIZE && !stop_buffering) {
		int16_t n=card.get();
		serial_char = (char)n;
		if(serial_char == '\n' || serial_char == '\r' || (serial_char == '#' && comment_mode == false) ||
	  	(serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1)||n==-1) {
		
			if(card.eof()) {
				SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
				stoptime=millis();
				char time[30];
				unsigned long t=(stoptime-starttime)/1000;
				int hours, minutes;
				minutes=(t/60)%60;
				hours=t/60/60;
				sprintf_P(time, PSTR("%i "MSG_END_HOUR" %i "MSG_END_MINUTE),hours, minutes);
				SERIAL_ECHO_START;
				SERIAL_ECHOLN(time);
				lcd_setstatus(time);
				card.printingHasFinished();
				card.checkautostart(true);
			}

			if(serial_char=='#')
				stop_buffering=true;

			if(!serial_count) {
				comment_mode = false; //for new command
				return; //if empty line
			}

			cmdbuffer[bufindw][serial_count] = 0; //terminate string

			//      if(!comment_mode){
			fromsd[bufindw] = true;
			buflen += 1;
			bufindw = (bufindw + 1)%BUFSIZE;
			//      }

			comment_mode = false; //for new command
			serial_count = 0; //clear buffer
		} else {
			if(serial_char == ';') 
				comment_mode = true;
			if(!comment_mode) 
				cmdbuffer[bufindw][serial_count++] = serial_char;
		}
	}
	#endif //SDSUPPORT
}


float code_value()
{
	return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL));
}

long code_value_long()
{
	return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
}

bool code_seen(char code)
{
	strchr_pointer = strchr(cmdbuffer[bufindr], code);
	return (strchr_pointer != NULL);  //Return True if a character was found
}

#define DEFINE_PGM_READ_ANY(type, reader)       \
    static inline type pgm_read_any(const type *p)  \
    { return pgm_read_##reader##_near(p); }

DEFINE_PGM_READ_ANY(float,       float);
DEFINE_PGM_READ_ANY(signed char, byte);

#define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
static const PROGMEM type array##_P[3] =        \
    { X_##CONFIG, Y_##CONFIG, Z_##CONFIG };     \
static inline type array(int axis)          \
    { return pgm_read_any(&array##_P[axis]); }

XYZ_CONSTS_FROM_CONFIG(float, base_min_pos,    MIN_POS);
XYZ_CONSTS_FROM_CONFIG(float, base_max_pos,    MAX_POS);
XYZ_CONSTS_FROM_CONFIG(float, base_home_pos,   HOME_POS);
XYZ_CONSTS_FROM_CONFIG(float, max_length,      MAX_LENGTH);
XYZ_CONSTS_FROM_CONFIG(float, home_retract_mm, HOME_RETRACT_MM);
XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);

#ifdef DUAL_X_CARRIAGE
  #if EXTRUDERS == 1 || defined(COREXY) \
      || !defined(X2_ENABLE_PIN) || !defined(X2_STEP_PIN) || !defined(X2_DIR_PIN) \
      || !defined(X2_HOME_POS) || !defined(X2_MIN_POS) || !defined(X2_MAX_POS) \
      || !defined(X_MAX_PIN) || X_MAX_PIN < 0
    #error "Missing or invalid definitions for DUAL_X_CARRIAGE mode."
  #endif
  #if X_HOME_DIR != -1 || X2_HOME_DIR != 1
    #error "Please use canonical x-carriage assignment" // the x-carriages are defined by their homing directions
  #endif

#define DXC_FULL_CONTROL_MODE 0
#define DXC_AUTO_PARK_MODE    1
#define DXC_DUPLICATION_MODE  2
static int dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;

static float x_home_pos(int extruder) {
	if (extruder == 0)
		return base_home_pos(X_AXIS) + add_homeing[X_AXIS];
	else
		// In dual carriage mode the extruder offset provides an override of the
		// second X-carriage offset when homed - otherwise X2_HOME_POS is used.
		// This allow soft recalibration of the second extruder offset position without firmware reflash
		// (through the M218 command).
		return (extruder_offset[X_AXIS][1] > 0) ? extruder_offset[X_AXIS][1] : X2_HOME_POS;
}

static int x_home_dir(int extruder) {
	return (extruder == 0) ? X_HOME_DIR : X2_HOME_DIR;
}

static float inactive_extruder_x_pos = X2_MAX_POS; // used in mode 0 & 1
static bool active_extruder_parked = false; // used in mode 1 & 2
static float raised_parked_position[NUM_AXIS]; // used in mode 1
static unsigned long delayed_move_time = 0; // used in mode 1
static float duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
static float duplicate_extruder_temp_offset = 0; // used in mode 2
bool extruder_duplication_enabled = false; // used in mode 2
#endif //DUAL_X_CARRIAGE

static void axis_is_at_home(int axis) {
#ifdef DUAL_X_CARRIAGE
	if (axis == X_AXIS) {
		if (active_extruder != 0) {
			current_position[X_AXIS] = x_home_pos(active_extruder);
			min_pos[X_AXIS] =          X2_MIN_POS;
			max_pos[X_AXIS] =          max(extruder_offset[X_AXIS][1], X2_MAX_POS);
			return;
		}
		else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0) {
			current_position[X_AXIS] = base_home_pos(X_AXIS) + add_homeing[X_AXIS];
			min_pos[X_AXIS] =          base_min_pos(X_AXIS) + add_homeing[X_AXIS];
			max_pos[X_AXIS] =          min(base_max_pos(X_AXIS) + add_homeing[X_AXIS],
			max(extruder_offset[X_AXIS][1], X2_MAX_POS) - duplicate_extruder_x_offset);
			return;
		}
  	}
#endif
	current_position[axis] = base_home_pos(axis) + add_homeing[axis];
	min_pos[axis] =          base_min_pos(axis) + add_homeing[axis];
	max_pos[axis] =          base_max_pos(axis) + add_homeing[axis];
}

#if defined(ENABLE_AUTO_BED_LEVELING) || defined(WITBOX)

static void do_blocking_move_to(float x, float y, float z) {
	float oldFeedRate = feedrate;

	feedrate = XY_TRAVEL_SPEED;

	current_position[X_AXIS] = x;
	current_position[Y_AXIS] = y;
	current_position[Z_AXIS] = z;
	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
	st_synchronize();

	feedrate = oldFeedRate;
}
static void do_blocking_extrude_to(float e) {
	float oldFeedRate = feedrate;

	feedrate = EXTRUSION_SPEED;

	current_position[E_AXIS] = e;
	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
	st_synchronize();

	feedrate = oldFeedRate;
}

#ifdef ENABLE_AUTO_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
{
	vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
	planeNormal.debug("planeNormal");
	plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
	//bedLevel.debug("bedLevel");

	//plan_bed_level_matrix.debug("bed level before");
	//vector_3 uncorrected_position = plan_get_position_mm();
	//uncorrected_position.debug("position before");

	vector_3 corrected_position = plan_get_position();
	//corrected_position.debug("position after");
	current_position[X_AXIS] = corrected_position.x;
	current_position[Y_AXIS] = corrected_position.y;
	current_position[Z_AXIS] = corrected_position.z;

	// but the bed at 0 so we don't go below it.
	current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure

	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}

#else // not AUTO_BED_LEVELING_GRID

static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
	plan_bed_level_matrix.set_to_identity();

	vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
	vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
	vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);

	vector_3 from_2_to_1 = (pt1 - pt2).get_normal();
	vector_3 from_2_to_3 = (pt3 - pt2).get_normal();
	vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal();
	planeNormal = vector_3(planeNormal.x, planeNormal.y, abs(planeNormal.z));

	plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);

	vector_3 corrected_position = plan_get_position();
	current_position[X_AXIS] = corrected_position.x;
	current_position[Y_AXIS] = corrected_position.y;
	current_position[Z_AXIS] = corrected_position.z;

	// put the bed at 0 so we don't go below it.
	current_position[Z_AXIS] = zprobe_zoffset;

	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}

#endif // AUTO_BED_LEVELING_GRID

static void run_z_probe() {
	plan_bed_level_matrix.set_to_identity();
	feedrate = homing_feedrate[Z_AXIS];

	// move down until you find the bed
	float zPosition = -10;
	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
	st_synchronize();

	// we have to let the planner know where we are right now as it is not where we said to go.
	zPosition = st_get_position_mm(Z_AXIS);
	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);

	// move up the retract distance
	zPosition += home_retract_mm(Z_AXIS);
	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
	st_synchronize();

	// move back down slowly to find bed
	feedrate = homing_feedrate[Z_AXIS]/4;
	zPosition -= home_retract_mm(Z_AXIS) * 2;
	plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
	st_synchronize();

	current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
	// make sure the planner knows where we are as it may be a bit different than we last said to move to
	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}

static void do_blocking_move_relative(float offset_x, float offset_y, float offset_z) {
	do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z);
}

static void setup_for_endstop_move() {
	saved_feedrate = feedrate;
	saved_feedmultiply = feedmultiply;
	feedmultiply = 100;
	previous_millis_cmd = millis();
	enable_endstops(true);
}

static void clean_up_after_endstop_move() {
#ifdef ENDSTOPS_ONLY_FOR_HOMING
	enable_endstops(false);
#endif

	feedrate = saved_feedrate;
	feedmultiply = saved_feedmultiply;
	previous_millis_cmd = millis();
}

/// Probe bed height at position (x,y), returns the measured z value
static float probe_pt(float x, float y, float z_before) {
	// move to right place
	do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
	do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);

	run_z_probe();
	float measured_z = current_position[Z_AXIS];

	SERIAL_PROTOCOLPGM(MSG_BED);
	SERIAL_PROTOCOLPGM(" x: ");
	SERIAL_PROTOCOL(x);
	SERIAL_PROTOCOLPGM(" y: ");
	SERIAL_PROTOCOL(y);
	SERIAL_PROTOCOLPGM(" z: ");
	SERIAL_PROTOCOL(measured_z);
	SERIAL_PROTOCOLPGM("\n");
	return measured_z;
}

#endif // #ifdef ENABLE_AUTO_BED_LEVELING
#endif  // #if defined(ENABLE_AUTO_BED_LEVELING) || defined(WITBOX)

static void homeaxis(int axis) {
	#define HOMEAXIS_DO(LETTER) ((LETTER##_MIN_PIN > -1 && LETTER##_HOME_DIR==-1) || (LETTER##_MAX_PIN > -1 && LETTER##_HOME_DIR==1))

	if (axis==X_AXIS ? HOMEAXIS_DO(X) : axis==Y_AXIS ? HOMEAXIS_DO(Y) : axis==Z_AXIS ? HOMEAXIS_DO(Z) : 0) {
		int axis_home_dir = home_dir(axis);
		#ifdef DUAL_X_CARRIAGE
			if (axis == X_AXIS)
				axis_home_dir = x_home_dir(active_extruder);
		#endif

		current_position[axis] = 0;
		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

		destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
		feedrate = homing_feedrate[axis];
		plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
		st_synchronize();

		current_position[axis] = 0;
		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
		destination[axis] = -home_retract_mm(axis) * axis_home_dir;
		plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
		st_synchronize();

		destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;

		feedrate = homing_feedrate[axis]/2 ;

		plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
		st_synchronize();

		axis_is_at_home(axis);
		destination[axis] = current_position[axis];
		feedrate = 0.0;
		endstops_hit_on_purpose();
		axis_known_position[axis] = true;
	}
}

#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
void refresh_cmd_timeout(void)
{
	previous_millis_cmd = millis();
}

void process_commands()
{
	unsigned long codenum; //throw away variable
	char *starpos = NULL;
	#ifdef ENABLE_AUTO_BED_LEVELING
  		float x_tmp, y_tmp, z_tmp, real_z;
	#endif // ENABLE_AUTO_BED_LEVELING
	if(code_seen('G')) {
		switch ((int)code_value()) {
    		case 0: // G0 -> G1
    		case 1: // G1
      			if(Stopped == false) {
				get_coordinates(); // For X Y Z E F
				prepare_move();
				}
				break;
    		case 2: // G2  - CW ARC
				if(Stopped == false) {
					get_arc_coordinates();
					prepare_arc_move(true);
				}
				break;
			case 3: // G3  - CCW ARC
				if(Stopped == false) {
					get_arc_coordinates();
					prepare_arc_move(false);
				}
				break;
    		case 4: // G4 dwell
      			LCD_MESSAGEPGM(MSG_DWELL);
      			codenum = 0;
      			if(code_seen('P')) codenum = code_value(); // milliseconds to wait
      			if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait

      			st_synchronize();
      			codenum += millis();  // keep track of when we started waiting
      			previous_millis_cmd = millis();
      			while(millis()  < codenum ){
					manage_heater();
					manage_inactivity();
      			}
      			break;
    		case 28: //G28 Home all Axis one at a time
    			lcd_disable_interrupt();
				#ifdef ENABLE_AUTO_BED_LEVELING
      				plan_bed_level_matrix.set_to_identity();  //Reset the plane ("erase" all leveling data)
				#endif //ENABLE_AUTO_BED_LEVELING

      			saved_feedrate = feedrate;
      			saved_feedmultiply = feedmultiply;
      			feedmultiply = 100;
      			previous_millis_cmd = millis();

      			enable_endstops(true);

      			for(int8_t i=0; i < NUM_AXIS; i++) {
					destination[i] = current_position[i];
      			}
      			feedrate = 0.0;

      			home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS])));

      			#if Z_HOME_DIR > 0                      // If homing away from BED do Z first
      				if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
						HOMEAXIS(Z);
     				}
				#endif

      			if((home_all_axis) || (code_seen(axis_codes[X_AXIS])))
      			{
      				#ifdef DUAL_X_CARRIAGE
						int tmp_extruder = active_extruder;
						extruder_duplication_enabled = false;
						active_extruder = !active_extruder;
						HOMEAXIS(X);
						inactive_extruder_x_pos = current_position[X_AXIS];
						active_extruder = tmp_extruder;
						HOMEAXIS(X);
						// reset state used by the different modes
						memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
						delayed_move_time = 0;
						active_extruder_parked = true;
      				#else
						HOMEAXIS(X);
      				#endif
      			}

      			if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
					HOMEAXIS(Y);
     			}

      			if(code_seen(axis_codes[X_AXIS]))
      			{
					if(code_value_long() != 0) {
	  					current_position[X_AXIS]=code_value()+add_homeing[0];
					}
      			}

      			if(code_seen(axis_codes[Y_AXIS])) {
					if(code_value_long() != 0) {
	  					current_position[Y_AXIS]=code_value()+add_homeing[1];
					}
      			}

      			#if Z_HOME_DIR < 0                      // If homing towards BED do Z last
				#ifndef Z_SAFE_HOMING
	  				if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
	    				#if defined (Z_RAISE_BEFORE_HOMING) && (Z_RAISE_BEFORE_HOMING > 0)
	      					destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
	      					feedrate = max_feedrate[Z_AXIS];
	      					plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
	      					st_synchronize();
	    				#endif
	    				HOMEAXIS(Z);
	  				}
				#else                      // Z Safe mode activated.
					if(home_all_axis) {
						destination[X_AXIS] = round(Z_SAFE_HOMING_X_POINT - X_PROBE_OFFSET_FROM_EXTRUDER);
	   					destination[Y_AXIS] = round(Z_SAFE_HOMING_Y_POINT - Y_PROBE_OFFSET_FROM_EXTRUDER);
	    				destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
	    				feedrate = XY_TRAVEL_SPEED;
	    				current_position[Z_AXIS] = 0;

	   					plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
	    				plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
	    				st_synchronize();
	    				current_position[X_AXIS] = destination[X_AXIS];
	    				current_position[Y_AXIS] = destination[Y_AXIS];

	    				HOMEAXIS(Z);
	  				}
					// Let's see if X and Y are homed and probe is inside bed area.
	  				if(code_seen(axis_codes[Z_AXIS])) {
	    				if ( (axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]) \
	      				&& (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER >= X_MIN_POS) \
	      				&& (current_position[X_AXIS]+X_PROBE_OFFSET_FROM_EXTRUDER <= X_MAX_POS) \
	      				&& (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER >= Y_MIN_POS) \
	      				&& (current_position[Y_AXIS]+Y_PROBE_OFFSET_FROM_EXTRUDER <= Y_MAX_POS)) {
	      					current_position[Z_AXIS] = 0;
	      					plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
	      					destination[Z_AXIS] = Z_RAISE_BEFORE_HOMING * home_dir(Z_AXIS) * (-1);    // Set destination away from bed
	      					feedrate = max_feedrate[Z_AXIS];
	      					plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
	      					st_synchronize();

							HOMEAXIS(Z);
	    				} else if (!((axis_known_position[X_AXIS]) && (axis_known_position[Y_AXIS]))) {
							LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
							SERIAL_ECHO_START;
							SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
	    				} else {
							LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
							SERIAL_ECHO_START;
							SERIAL_ECHOLNPGM(MSG_ZPROBE_OUT);
	    				}
	  				}
				#endif
      			#endif



      			if(code_seen(axis_codes[Z_AXIS])) {
					if(code_value_long() != 0) {
	  					current_position[Z_AXIS]=code_value()+add_homeing[2];
					}
      			}
      			#ifdef ENABLE_AUTO_BED_LEVELING
					if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
	  					current_position[Z_AXIS] += zprobe_zoffset;  //Add Z_Probe offset (the distance is negative)
					}
      			#endif
      			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

      			#ifdef ENDSTOPS_ONLY_FOR_HOMING
					enable_endstops(false);
      			#endif

      			feedrate = saved_feedrate;
      			feedmultiply = saved_feedmultiply;
      			previous_millis_cmd = millis();
      			endstops_hit_on_purpose();
      			lcd_enable_interrupt();
      			break;

			#ifdef ENABLE_AUTO_BED_LEVELING
    		case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
			{
	    		#if Z_MIN_PIN == -1
	    			#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
	    		#endif

	    		// Prevent user from running a G29 without first homing in X and Y
	    		if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
	    		{
					LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
					SERIAL_ECHO_START;
					SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
					break; // abort G29, since we don't know where we are
	    		}

	    		st_synchronize();
	    		// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
	    		//vector_3 corrected_position = plan_get_position_mm();
	    		//corrected_position.debug("position before G29");
	    		plan_bed_level_matrix.set_to_identity();
	    		vector_3 uncorrected_position = plan_get_position();
	   			//uncorrected_position.debug("position durring G29");
	    		current_position[X_AXIS] = uncorrected_position.x;
	    		current_position[Y_AXIS] = uncorrected_position.y;
	    		current_position[Z_AXIS] = uncorrected_position.z;
	    		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
	    		setup_for_endstop_move();

	    		feedrate = homing_feedrate[Z_AXIS];
				#ifdef AUTO_BED_LEVELING_GRID
	    			// probe at the points of a lattice grid

	    			int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
	    			int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);

	    			// solve the plane equation ax + by + d = z
	    			// A is the matrix with rows [x y 1] for all the probed points
	    			// B is the vector of the Z positions
	    			// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
	    			// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z

	    			// "A" matrix of the linear system of equations
	    			double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3];
	    			// "B" vector of Z points
	    			double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS];


	    			int probePointCounter = 0;
	    			bool zig = true;

	    			for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
	    			{
	      				int xProbe, xInc;
	      				if (zig)
	      				{
							xProbe = LEFT_PROBE_BED_POSITION;
							//xEnd = RIGHT_PROBE_BED_POSITION;
							xInc = xGridSpacing;
							zig = false;
	      				} else // zag
	      				{
							xProbe = RIGHT_PROBE_BED_POSITION;
							//xEnd = LEFT_PROBE_BED_POSITION;
							xInc = -xGridSpacing;
							zig = true;
	      				}

	      				for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++)
	      				{
							float z_before;
							if (probePointCounter == 0)
							{
		  						// raise before probing
		  						z_before = Z_RAISE_BEFORE_PROBING;
							} else
							{
		  						// raise extruder
		  						z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
							}


							float measured_z = probe_pt(xProbe, yProbe, z_before);

							eqnBVector[probePointCounter] = measured_z;

							eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe;
							eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe;
							eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1;
							probePointCounter++;
							xProbe += xInc;
	      				}
	    			}
	   				clean_up_after_endstop_move();

	    			// solve lsq problem
	    			double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector);

	    			SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
	    			SERIAL_PROTOCOL(plane_equation_coefficients[0]);
	    			SERIAL_PROTOCOLPGM(" b: ");
	    			SERIAL_PROTOCOL(plane_equation_coefficients[1]);
	    			SERIAL_PROTOCOLPGM(" d: ");
	    			SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);

	    			set_bed_level_equation_lsq(plane_equation_coefficients);

	    			free(plane_equation_coefficients);

				#else // AUTO_BED_LEVELING_GRID not defined

	    			// Probe at 3 arbitrary points
	    			// probe 1
	    			float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);

	    			// probe 2
	    			float z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);

	    			// probe 3
	    			float z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);

	    			clean_up_after_endstop_move();

	    			set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);


				#endif // AUTO_BED_LEVELING_GRID
	    		st_synchronize();

	    		// The following code correct the Z height difference from z-probe position and hotend tip position.
	    		// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
	    		// When the bed is uneven, this height must be corrected.
	    		real_z = float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS];  //get the real Z (since the auto bed leveling is already correcting the plane)
	    		x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
	    		y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
	    		z_tmp = current_position[Z_AXIS];

	    		apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp);         //Apply the correction sending the probe offset
	    		current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS];   //The difference is added to current position and sent to planner.
	    		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
			}
			break;

    		case 30: // G30 Single Z Probe
			{
	    		st_synchronize();
	    		// TODO: make sure the bed_level_rotation_matrix is identity or the planner will get set incorectly
	    		setup_for_endstop_move();
	    		feedrate = homing_feedrate[Z_AXIS];

	    		run_z_probe();
	    		SERIAL_PROTOCOLPGM(MSG_BED);
	    		SERIAL_PROTOCOLPGM(" X: ");
	    		SERIAL_PROTOCOL(current_position[X_AXIS]);
	    		SERIAL_PROTOCOLPGM(" Y: ");
	    		SERIAL_PROTOCOL(current_position[Y_AXIS]);
	    		SERIAL_PROTOCOLPGM(" Z: ");
	    		SERIAL_PROTOCOL(current_position[Z_AXIS]);
	    		SERIAL_PROTOCOLPGM("\n");

	    		clean_up_after_endstop_move();
			}
			break;
			#endif // ENABLE_AUTO_BED_LEVELING
    		case 90: // G90
      			relative_mode = false;
      			break;
    		case 91: // G91
      			relative_mode = true;
      			break;
    		case 92: // G92
      			if(!code_seen(axis_codes[E_AXIS]))
					st_synchronize();
      			for(int8_t i=0; i < NUM_AXIS; i++) {
					if(code_seen(axis_codes[i])) {
	   					if(i == E_AXIS) {
	     					current_position[i] = code_value();
	     					plan_set_e_position(current_position[E_AXIS]);
	   					}
	   					else {
	     					current_position[i] = code_value()+add_homeing[i];
	     					plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
	   					}
					}
      			}
      			break;
    	}
  	}
	else if(code_seen('M'))
	{
		switch( (int)code_value() )
		{
			#ifdef ULTIPANEL
			case 0: // M0 - Unconditional stop - Wait for user button press on LCD
			case 1: // M1 - Conditional stop - Wait for user button press on LCD
			{
				lcd_disable_button();
				LCD_MESSAGEPGM(MSG_USERWAIT);
				draw_status_screen();
				lcd_update(true);

				codenum = 0;
				if(code_seen('P')) codenum = code_value(); // milliseconds to wait
				if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait

				st_synchronize();
				previous_millis_cmd = millis();
				if (codenum > 0){
					codenum += millis();  // keep track of when we started waiting
					lcd_enable_button();
					while(millis()  < codenum && !LCD_CLICKED) {
						manage_heater();
					}
					lcd_disable_button();
				} else {
					lcd_enable_button();
					while(!LCD_CLICKED){
						manage_heater();
					}
					lcd_disable_button();
				}
				LCD_MESSAGEPGM(MSG_PRINTING);
				lcd_update();
				lcd_enable_button();
			}
    		break;
			#endif
    		case 17:
				LCD_MESSAGEPGM(MSG_NO_MOVE);
				enable_x();
				enable_y();
				enable_z();
				enable_e0();
				enable_e1();
				enable_e2();
      			break;

			#ifdef SDSUPPORT
    		case 20: // M20 - list SD card
				SERIAL_PROTOCOLLNPGM(MSG_BEGIN_FILE_LIST);
				card.ls();
				SERIAL_PROTOCOLLNPGM(MSG_END_FILE_LIST);
      			break;
    		case 21: // M21 - init SD card
      			card.initsd();
      			break;
			case 22: //M22 - release SD card
				card.release();
				break;
			case 23: //M23 - Select file
				starpos = (strchr(strchr_pointer + 4,'*'));
				if(starpos!=NULL)
					*(starpos)='\0';
				card.openFile(strchr_pointer + 4,true);
				break;
			case 24: //M24 - Start SD print
				card.startFileprint();
				starttime=millis();
				feedmultiply = 100;
				break;

    		case 25: //M25 - Pause SD print
				float target[4];
				float lastpos[4];

				target[X_AXIS]=current_position[X_AXIS];
				target[Y_AXIS]=current_position[Y_AXIS];
				target[Z_AXIS]=current_position[Z_AXIS];
				target[E_AXIS]=current_position[E_AXIS];
      
				lastpos[X_AXIS]=current_position[X_AXIS];
				lastpos[Y_AXIS]=current_position[Y_AXIS];
				lastpos[Z_AXIS]=current_position[Z_AXIS];
				lastpos[E_AXIS]=current_position[E_AXIS];

				plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);

				target[Z_AXIS]+= FILAMENTCHANGE_ZADD;
				plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);

				#if X_MAX_POS < 250
					target[X_AXIS]= 0 ;
					target[Y_AXIS]= 150 ;
				#else
					target[X_AXIS]= X_MAX_POS - 5 ;
					target[Y_AXIS]= Y_MAX_POS - 5 ;
				#endif

				plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);

      			st_synchronize();

				LCD_MESSAGEPGM(MSG_PAUSED);
				lcd_update();
				lcd_enable_button();

      			while(!LCD_CLICKED){
					lcd_update();
					manage_heater();
					plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],current_position[E_AXIS], 300/60, active_extruder);
					st_synchronize();
				}
      
				lcd_disable_button();
				LCD_MESSAGEPGM(MSG_PRINTING);
				lcd_update();

				plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //should do nothing
				plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //move xy back
				plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder); //move z back
				plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //final untretract
				st_synchronize();

				lcd_enable_button();
				stop_buffer = false;
				break;

			case 26: //M26 - Set SD index
				if(card.cardOK && code_seen('S')) {
					card.setIndex(code_value_long());
				}
				break;
			case 27: //M27 - Get SD status
				card.getStatus();
				break;
			case 28: //M28 - Start SD write
				starpos = (strchr(strchr_pointer + 4,'*'));
				if(starpos != NULL){
					char* npos = strchr(cmdbuffer[bufindr], 'N');
					strchr_pointer = strchr(npos,' ') + 1;
					*(starpos) = '\0';
				}
				card.openFile(strchr_pointer+4,false);
				break;
			case 29: //M29 - Stop SD write
				//processed in write to file routine above
				//card,saving = false;
				break;
			case 30: //M30 <filename> Delete File
				if (card.cardOK){
					card.closefile();
					starpos = (strchr(strchr_pointer + 4,'*'));
					if(starpos != NULL){
						char* npos = strchr(cmdbuffer[bufindr], 'N');
						strchr_pointer = strchr(npos,' ') + 1;
						*(starpos) = '\0';
					}
					card.removeFile(strchr_pointer + 4);
				}
				break;
			case 32: //M32 - Select file and start SD print
			{
				if(card.sdprinting) {
					st_synchronize();
				}
				starpos = (strchr(strchr_pointer + 4,'*'));

				char* namestartpos = (strchr(strchr_pointer + 4,'!'));   //find ! to indicate filename string start.
				if(namestartpos==NULL)
				{
					namestartpos=strchr_pointer + 4; //default name position, 4 letters after the M
				}
				else
					namestartpos++; //to skip the '!'

				if(starpos!=NULL)
					*(starpos)='\0';

				bool call_procedure=(code_seen('P'));

				if(strchr_pointer>namestartpos)
					call_procedure=false;  //false alert, 'P' found within filename

				if( card.cardOK )
				{
					card.openFile(namestartpos,true,!call_procedure);
					if(code_seen('S'))
						if(strchr_pointer<namestartpos) //only if "S" is occuring _before_ the filename
							card.setIndex(code_value_long());
					card.startFileprint();
					if(!call_procedure)
						starttime=millis(); //procedure calls count as normal print time.
				}
			} break;
			case 928: //M928 - Start SD write
				starpos = (strchr(strchr_pointer + 5,'*'));
				if(starpos != NULL){
					char* npos = strchr(cmdbuffer[bufindr], 'N');
					strchr_pointer = strchr(npos,' ') + 1;
					*(starpos) = '\0';
				}
				card.openLogFile(strchr_pointer+5);
      			break;
			#endif //SDSUPPORT

			case 31: //M31 take time since the start of the SD print or an M109 command
			{
				stoptime=millis();
				char time[30];
				unsigned long t=(stoptime-starttime)/1000;
				int sec,min;
				min=t/60;
				sec=t%60;
				sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
				SERIAL_ECHO_START;
				SERIAL_ECHOLN(time);
				lcd_setstatus(time);
				autotempShutdown();
			}
			break;
			case 42: //M42 -Change pin status via gcode
				if (code_seen('S'))
				{
					int pin_status = code_value();
					int pin_number = LED_PIN;
					if (code_seen('P') && pin_status >= 0 && pin_status <= 255)
						pin_number = code_value();
					for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
					{
						if (sensitive_pins[i] == pin_number)
						{
							pin_number = -1;
							break;
						}
					}
					#if defined(FAN_PIN) && FAN_PIN > -1
					if (pin_number == FAN_PIN)
	  					fanSpeed = pin_status;
      				#endif
					if (pin_number > -1)
					{
	  					pinMode(pin_number, OUTPUT);
						digitalWrite(pin_number, pin_status);
						analogWrite(pin_number, pin_status);
					}
				}
				break;
    		case 104: // M104
      			if(setTargetedHotend(104)){
					break;
      			}
      			if (code_seen('S')) setTargetHotend(code_value(), tmp_extruder);
					#ifdef DUAL_X_CARRIAGE
      					if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
							setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
					#endif
      			setWatch();
      			break;
    		case 140: // M140 set bed temp
				if (code_seen('S')) setTargetBed(code_value());
					break;
			case 105 : // M105
				if(setTargetedHotend(105)){
					break;
				}
				#if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
					SERIAL_PROTOCOLPGM("ok T:");
					SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
					SERIAL_PROTOCOLPGM(" /");
					SERIAL_PROTOCOL_F(degTargetHotend(tmp_extruder),1);
					#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
						SERIAL_PROTOCOLPGM(" B:");
						SERIAL_PROTOCOL_F(degBed(),1);
						SERIAL_PROTOCOLPGM(" /");
						SERIAL_PROTOCOL_F(degTargetBed(),1);
					#endif //TEMP_BED_PIN
					for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
	  					SERIAL_PROTOCOLPGM(" T");
	  					SERIAL_PROTOCOL(cur_extruder);
	  					SERIAL_PROTOCOLPGM(":");
	  					SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
	  					SERIAL_PROTOCOLPGM(" /");
	  					SERIAL_PROTOCOL_F(degTargetHotend(cur_extruder),1);
					}
      			#else
					SERIAL_ERROR_START;
					SERIAL_ERRORLNPGM(MSG_ERR_NO_THERMISTORS);
      			#endif

				SERIAL_PROTOCOLPGM(" @:");
      			#ifdef EXTRUDER_WATTS
					SERIAL_PROTOCOL((EXTRUDER_WATTS * getHeaterPower(tmp_extruder))/127);
					SERIAL_PROTOCOLPGM("W");
      			#else
					SERIAL_PROTOCOL(getHeaterPower(tmp_extruder));
      			#endif

				SERIAL_PROTOCOLPGM(" B@:");
      			#ifdef BED_WATTS
					SERIAL_PROTOCOL((BED_WATTS * getHeaterPower(-1))/127);
					SERIAL_PROTOCOLPGM("W");
      			#else
					SERIAL_PROTOCOL(getHeaterPower(-1));
      			#endif

				#ifdef SHOW_TEMP_ADC_VALUES
	  				#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
	    				SERIAL_PROTOCOLPGM("    ADC B:");
	    				SERIAL_PROTOCOL_F(degBed(),1);
	    				SERIAL_PROTOCOLPGM("C->");
	    				SERIAL_PROTOCOL_F(rawBedTemp()/OVERSAMPLENR,0);
	  				#endif
	  				for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
	    				SERIAL_PROTOCOLPGM("  T");
	    				SERIAL_PROTOCOL(cur_extruder);
	    				SERIAL_PROTOCOLPGM(":");
	    				SERIAL_PROTOCOL_F(degHotend(cur_extruder),1);
	    				SERIAL_PROTOCOLPGM("C->");
	    				SERIAL_PROTOCOL_F(rawHotendTemp(cur_extruder)/OVERSAMPLENR,0);
	  				}
				#endif

				SERIAL_PROTOCOLLN("");
      			return;
      			break;
			case 109:
			{// M109 - Wait for extruder heater to reach target.
      			if(setTargetedHotend(109)){
        			break;
				}
				LCD_MESSAGEPGM(MSG_HEATING_PROCESS);
				lcd_update();
      
				#ifdef AUTOTEMP
					autotemp_enabled=false;
				#endif
				if (code_seen('S')) {
					setTargetHotend(code_value(), tmp_extruder);
					#ifdef DUAL_X_CARRIAGE
					if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
						setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
					#endif
					CooldownNoWait = true;
				} else if (code_seen('R')) {
					setTargetHotend(code_value(), tmp_extruder);
					#ifdef DUAL_X_CARRIAGE
						if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && tmp_extruder == 0)
							setTargetHotend1(code_value() == 0.0 ? 0.0 : code_value() + duplicate_extruder_temp_offset);
					#endif
					CooldownNoWait = false;
				}

				#ifdef AUTOTEMP
					if (code_seen('S')) autotemp_min=code_value();
					if (code_seen('B')) autotemp_max=code_value();
					if (code_seen('F')) {
						autotemp_factor=code_value();
						autotemp_enabled=true;
					}
				#endif

				setWatch();
				codenum = millis();

				/* See if we are heating up or cooling down */
				target_direction = isHeatingHotend(tmp_extruder); // true if heating, false if cooling

				cancel_heatup = false;

				#ifdef TEMP_RESIDENCY_TIME
				long residencyStart;
				residencyStart = -1;
				/* continue to loop until we have reached the target temp
				_and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
				while((!cancel_heatup)&&((residencyStart == -1) || (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) ) {
				#else
					while ( target_direction ? (isHeatingHotend(tmp_extruder)) : (isCoolingHotend(tmp_extruder)&&(CooldownNoWait==false)) ) {
				#endif //TEMP_RESIDENCY_TIME
	  					if( (millis() - codenum) > 1000UL )
	  					{ //Print Temp Reading and remaining time every 1 second while heating up/cooling down
	    					SERIAL_PROTOCOLPGM("T:");
	    					SERIAL_PROTOCOL_F(degHotend(tmp_extruder),1);
	    					SERIAL_PROTOCOLPGM(" E:");
	    					SERIAL_PROTOCOL((int)tmp_extruder);
	    					#ifdef TEMP_RESIDENCY_TIME
	      						SERIAL_PROTOCOLPGM(" W:");
	      						if(residencyStart > -1)
	      						{
		 							codenum = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
		 							SERIAL_PROTOCOLLN( codenum );
	      						}
	      						else
	      						{
		 							SERIAL_PROTOCOLLN( "?" );
	      						}
	    					#else
	      						SERIAL_PROTOCOLLN("");
	    					#endif
	    					codenum = millis();
	  					}
      					manage_heater();
      					manage_inactivity();
      					lcd_update();

						#ifdef TEMP_RESIDENCY_TIME
          					/* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
          					or when current temp falls outside the hysteresis after target temp was reached */
          					if ((residencyStart == -1 &&  target_direction && (degHotend(tmp_extruder) >= (degTargetHotend(tmp_extruder)-TEMP_WINDOW))) ||
          					(residencyStart == -1 && !target_direction && (degHotend(tmp_extruder) <= (degTargetHotend(tmp_extruder)+TEMP_WINDOW))) ||
          					(residencyStart > -1 && labs(degHotend(tmp_extruder) - degTargetHotend(tmp_extruder)) > TEMP_HYSTERESIS) )
          					{
								residencyStart = millis();
							}
						#endif //TEMP_RESIDENCY_TIME
					}
        			LCD_MESSAGEPGM(MSG_HEATING_DONE);
        			lcd_update();

        			starttime=millis();
        			previous_millis_cmd = millis();
      			}

      			if (card.sdprinting == true){
					LCD_MESSAGEPGM(MSG_PRINTING);
					lcd_update();
				}
				break;

			case 190: // M190 - Wait for bed heater to reach target.
				#if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
					LCD_MESSAGEPGM(MSG_BED_HEATING_PROCESS);
					if (code_seen('S')) {
						setTargetBed(code_value());
						CooldownNoWait = true;
					} else if (code_seen('R')) {
						setTargetBed(code_value());
						CooldownNoWait = false;
					}
					codenum = millis();

					cancel_heatup = false;
					target_direction = isHeatingBed(); // true if heating, false if cooling

					while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) )
					{
						if(( millis() - codenum) > 1000 ) //Print Temp Reading every 1 second while heating up.
						{
							float tt=degHotend(active_extruder);
							SERIAL_PROTOCOLPGM("T:");
							SERIAL_PROTOCOL(tt);
							SERIAL_PROTOCOLPGM(" E:");
							SERIAL_PROTOCOL((int)active_extruder);
							SERIAL_PROTOCOLPGM(" B:");
							SERIAL_PROTOCOL_F(degBed(),1);
							SERIAL_PROTOCOLLN("");
							codenum = millis();
						}
						manage_heater();
						manage_inactivity();
						lcd_update();
					}
					LCD_MESSAGEPGM(MSG_BED_HEATING_DONE);
					previous_millis_cmd = millis();
    			#endif
				break;

    		#if defined(FAN_PIN) && FAN_PIN > -1
      		case 106: //M106 Fan On
				if (code_seen('S')){
	   				fanSpeed=constrain(code_value(),0,255);
				}
				else {
					fanSpeed=255;
				}
				break;
			case 107: //M107 Fan Off
				fanSpeed = 0;
				break;
			#endif //FAN_PIN

			#if defined(PS_ON_PIN) && PS_ON_PIN > -1
			case 80: // M80 - Turn on Power Supply
				SET_OUTPUT(PS_ON_PIN); //GND
				WRITE(PS_ON_PIN, PS_ON_AWAKE);

				// If you have a switch on suicide pin, this is useful
				// if you want to start another print with suicide feature after
				// a print without suicide...
				#if defined SUICIDE_PIN && SUICIDE_PIN > -1
					SET_OUTPUT(SUICIDE_PIN);
					WRITE(SUICIDE_PIN, HIGH);
				#endif

				#ifdef ULTIPANEL
					powersupply = true;
					LCD_MESSAGEPGM(WELCOME_MSG);
				#endif
				break;
			#endif
			case 81: // M81 - Turn off Power Supply
				disable_heater();
				st_synchronize();
				disable_e0();
				disable_e1();
				disable_e2();
				finishAndDisableSteppers();
				fanSpeed = 0;
				delay(1000); // Wait a little before to switch off
      			#if defined(SUICIDE_PIN) && SUICIDE_PIN > -1
					st_synchronize();
					suicide();
				#elif defined(PS_ON_PIN) && PS_ON_PIN > -1
					SET_OUTPUT(PS_ON_PIN);
					WRITE(PS_ON_PIN, PS_ON_ASLEEP);
				#endif
				#ifdef ULTIPANEL
					powersupply = false;
					LCD_MESSAGEPGM(MACHINE_NAME" "MSG_OFF".");
				#endif
	  			break;
			case 82:
				axis_relative_modes[3] = false;
				break;
			case 83:
				axis_relative_modes[3] = true;
				break;
			case 18: //compatibility
			case 84: // M84
				if(code_seen('S')){
					stepper_inactive_time = code_value() * 1000;
				}
				else
				{
					bool all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS]))|| (code_seen(axis_codes[E_AXIS])));
					if(all_axis)
					{
	  					st_synchronize();
	  					disable_e0();
	  					disable_e1();
	  					disable_e2();
	  					finishAndDisableSteppers();
					}
					else
					{
						st_synchronize();
						if(code_seen('X')) disable_x();
						if(code_seen('Y')) disable_y();
						if(code_seen('Z')) disable_z();
						#if ((E0_ENABLE_PIN != X_ENABLE_PIN) && (E1_ENABLE_PIN != Y_ENABLE_PIN)) // Only enable on boards that have seperate ENABLE_PINS
						if(code_seen('E')) {
							disable_e0();
							disable_e1();
							disable_e2();
						}
						#endif
					}
      			}
      			break;
    		case 85: // M85
      			code_seen('S');
      			max_inactive_time = code_value() * 1000;
      			break;
    		case 92: // M92
      			for(int8_t i=0; i < NUM_AXIS; i++)
      			{
					if(code_seen(axis_codes[i]))
					{
	  					if(i == 3) { // E
	    					float value = code_value();
	    					if(value < 20.0) {
	      						float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
	      						max_e_jerk *= factor;
	      						max_feedrate[i] *= factor;
	      						axis_steps_per_sqr_second[i] *= factor;
	    					}
	    					axis_steps_per_unit[i] = value;
	  					}
	  					else {
	    					axis_steps_per_unit[i] = code_value();
	  					}
					}
      			}
      			break;
    		case 115: // M115
      			SERIAL_PROTOCOLPGM(MSG_M115_REPORT);
      			break;
			case 117: // M117 display message
			#ifdef GCODE_MESSAGES_ENABLE
				starpos = (strchr(strchr_pointer + 5,'*'));
				if(starpos!=NULL)
				*(starpos)='\0';
				lcd_setstatus(strchr_pointer + 5);
			#endif // GCODE_MESSAGES_ENABLE
				break;

			case 114: // M114
				SERIAL_PROTOCOLPGM("X:");
      			SERIAL_PROTOCOL(current_position[X_AXIS]);
      			SERIAL_PROTOCOLPGM(" Y:");
      			SERIAL_PROTOCOL(current_position[Y_AXIS]);
      			SERIAL_PROTOCOLPGM(" Z:");
      			SERIAL_PROTOCOL(current_position[Z_AXIS]);
      			SERIAL_PROTOCOLPGM(" E:");
      			SERIAL_PROTOCOL(current_position[E_AXIS]);

      			SERIAL_PROTOCOLPGM(MSG_COUNT_X);
      			SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
      			SERIAL_PROTOCOLPGM(" Y:");
      			SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
      			SERIAL_PROTOCOLPGM(" Z:");
      			SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);

      			SERIAL_PROTOCOLLN("");
      			break;
    		case 120: // M120
      			enable_endstops(false) ;
      			break;
    		case 121: // M121
      			enable_endstops(true) ;
      			break;
    		case 119: // M119
    			SERIAL_PROTOCOLLN(MSG_M119_REPORT);
      			#if defined(X_MIN_PIN) && X_MIN_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_X_MIN);
					SERIAL_PROTOCOLLN(((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			#if defined(X_MAX_PIN) && X_MAX_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_X_MAX);
					SERIAL_PROTOCOLLN(((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			#if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_Y_MIN);
					SERIAL_PROTOCOLLN(((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			#if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_Y_MAX);
					SERIAL_PROTOCOLLN(((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			#if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_Z_MIN);
					SERIAL_PROTOCOLLN(((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			#if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
					SERIAL_PROTOCOLPGM(MSG_Z_MAX);
					SERIAL_PROTOCOLLN(((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?MSG_ENDSTOP_HIT:MSG_ENDSTOP_OPEN));
      			#endif
      			break;
			//TODO: update for all axis, use for loop
			case 200: // M200 D<millimeters> set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
			{
				float area = .0;
				float radius = .0;
				if(code_seen('D')) {
					radius = (float)code_value() * .5;
					if(radius == 0) {
						area = 1;
					} else {
						area = M_PI * pow(radius, 2);
					}
				} else {
					//reserved for setting filament diameter via UFID or filament measuring device
					break;
				}
				tmp_extruder = active_extruder;
				if(code_seen('T')) {
					tmp_extruder = code_value();
					if(tmp_extruder >= EXTRUDERS) {
						SERIAL_ECHO_START;
						SERIAL_ECHO(MSG_M200_INVALID_EXTRUDER);
					}
					SERIAL_ECHOLN(tmp_extruder);
					break;
				}
				volumetric_multiplier[tmp_extruder] = 1 / area;
			}
      		break;
			case 201: // M201
				for(int8_t i=0; i < NUM_AXIS; i++)
				{
					if(code_seen(axis_codes[i]))
					{
						max_acceleration_units_per_sq_second[i] = code_value();
					}
				}
				// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
				reset_acceleration_rates();
				break;
			case 203: // M203 max feedrate mm/sec
				for(int8_t i=0; i < NUM_AXIS; i++) {
					if(code_seen(axis_codes[i])) max_feedrate[i] = code_value();
				}
				break;
			case 204: // M204 acclereration S normal moves T filmanent only moves
			{
				if(code_seen('S')) acceleration = code_value() ;
				if(code_seen('T')) retract_acceleration = code_value() ;
			}
			break;
			case 205: //M205 advanced settings:  minimum travel speed S=while printing T=travel only,  B=minimum segment time X= maximum xy jerk, Z=maximum Z jerk
			{
				if(code_seen('S')) minimumfeedrate = code_value();
				if(code_seen('T')) mintravelfeedrate = code_value();
				if(code_seen('B')) minsegmenttime = code_value() ;
				if(code_seen('X')) max_xy_jerk = code_value() ;
				if(code_seen('Z')) max_z_jerk = code_value() ;
				if(code_seen('E')) max_e_jerk = code_value() ;
			}
			break;
			case 206: // M206 additional homeing offset
				for(int8_t i=0; i < 3; i++)
				{
					if(code_seen(axis_codes[i])) add_homeing[i] = code_value();
				}
			break;
    		#if EXTRUDERS > 1
    		case 218: // M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
			{
				if(setTargetedHotend(218)){
					break;
				}
				if(code_seen('X'))
				{
					extruder_offset[X_AXIS][tmp_extruder] = code_value();
				}
				if(code_seen('Y'))
				{
					extruder_offset[Y_AXIS][tmp_extruder] = code_value();
				}
				#ifdef DUAL_X_CARRIAGE
				if(code_seen('Z'))
 				{
					extruder_offset[Z_AXIS][tmp_extruder] = code_value();
				}
				#endif
				SERIAL_ECHO_START;
				SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
				for(tmp_extruder = 0; tmp_extruder < EXTRUDERS; tmp_extruder++)
				{
					SERIAL_ECHO(" ");
					SERIAL_ECHO(extruder_offset[X_AXIS][tmp_extruder]);
					SERIAL_ECHO(",");
					SERIAL_ECHO(extruder_offset[Y_AXIS][tmp_extruder]);
					#ifdef DUAL_X_CARRIAGE
					SERIAL_ECHO(",");
					SERIAL_ECHO(extruder_offset[Z_AXIS][tmp_extruder]);
					#endif
				}
				SERIAL_ECHOLN("");
			}break;
    		#endif
    		case 220: // M220 S<factor in percent>- set speed factor override percentage
    		{
      			if(code_seen('S'))
      			{
					feedmultiply = code_value() ;
      			}
    		}
    		break;
    		case 221: // M221 S<factor in percent>- set extrude factor override percentage
    		{
      			if(code_seen('S'))
      			{
					extrudemultiply = code_value() ;
      			}
    		}
    		break;
			case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
			{
      			if(code_seen('P')){
					int pin_number = code_value(); // pin number
					int pin_state = -1; // required pin state - default is inverted

					if(code_seen('S')) pin_state = code_value(); // required pin state

					if(pin_state >= -1 && pin_state <= 1){
						for(int8_t i = 0; i < (int8_t)sizeof(sensitive_pins); i++)
						{
							if (sensitive_pins[i] == pin_number)
							{
								pin_number = -1;
								break;
							}
						}

						if (pin_number > -1)
						{
							st_synchronize();
							pinMode(pin_number, INPUT);
							int target;

							switch(pin_state){
								case 1:
	      							target = HIGH;
	      							break;
	    						case 0:
	      							target = LOW;
	      							break;
	    						case -1:
	      							target = !digitalRead(pin_number);
	      							break;
	    					}

	    					while(digitalRead(pin_number) != target){
	      						manage_heater();
	      						manage_inactivity();
							}
						}
					}
      			}
    		}
    		break;

#if (LARGE_FLASH == true && ( BEEPER > 0 || defined(ULTRALCD)))
            case 300: // M300
            {
                int beepS = code_seen('S') ? code_value() : 110;
                int beepP = code_seen('P') ? code_value() : 1000;

                if (beepS > 4000) {
                    beepS = 4000;
                } else if (beepS < 16) {
                    _delay_ms(beepP);
                    break;
                }

                lcd_beep_hz_ms(beepS, beepP);
            }
                break;
#endif // (LARGE_FLASH == true && ( BEEPER > 0 || defined(ULTRALCD)))

#ifdef PIDTEMP
    		case 301: // M301
      		{
				if(code_seen('P')) Kp = code_value();
				if(code_seen('I')) Ki = scalePID_i(code_value());
				if(code_seen('D')) Kd = scalePID_d(code_value());

				#ifdef PID_ADD_EXTRUSION_RATE
					if(code_seen('C')) Kc = code_value();
				#endif

				updatePID();
				SERIAL_PROTOCOL(MSG_OK);
				SERIAL_PROTOCOL(" p:");
				SERIAL_PROTOCOL(Kp);
				SERIAL_PROTOCOL(" i:");
				SERIAL_PROTOCOL(unscalePID_i(Ki));
				SERIAL_PROTOCOL(" d:");
				SERIAL_PROTOCOL(unscalePID_d(Kd));
				#ifdef PID_ADD_EXTRUSION_RATE
					SERIAL_PROTOCOL(" c:");
					//Kc does not have scaling applied above, or in resetting defaults
					SERIAL_PROTOCOL(Kc);
				#endif
				SERIAL_PROTOCOLLN("");
      		}
      		break;
    		#endif //PIDTEMP
    		#ifdef PIDTEMPBED
    		case 304: // M304
      		{
				if(code_seen('P')) bedKp = code_value();
				if(code_seen('I')) bedKi = scalePID_i(code_value());
				if(code_seen('D')) bedKd = scalePID_d(code_value());

				updatePID();
				SERIAL_PROTOCOL(MSG_OK);
				SERIAL_PROTOCOL(" p:");
				SERIAL_PROTOCOL(bedKp);
				SERIAL_PROTOCOL(" i:");
				SERIAL_PROTOCOL(unscalePID_i(bedKi));
				SERIAL_PROTOCOL(" d:");
				SERIAL_PROTOCOL(unscalePID_d(bedKd));
				SERIAL_PROTOCOLLN("");
      		}
      		break;
    		#endif //PIDTEMP
			#ifdef DOGLCD
    		case 250: // M250  Set LCD contrast value: C<value> (value 0..63)
     		{
	  			if (code_seen('C')) {
	   				lcd_setcontrast( ((int)code_value())&63 );
	  			}
	  			SERIAL_PROTOCOLPGM("lcd contrast value: ");
	  			SERIAL_PROTOCOL(lcd_contrast);
	  			SERIAL_PROTOCOLLN("");
     		}
    		break;
			#endif
    		#ifdef PREVENT_DANGEROUS_EXTRUDE
    		case 302: // allow cold extrudes, or set the minimum extrude temperature
    		{
	  			float temp = .0;
	  			if (code_seen('S')) temp=code_value();
      			set_extrude_min_temp(temp);
    		}
    		break;
			#endif
    		case 303: // M303 PID autotune
    		{
      			float temp = 150.0;
      			int e=0;
      			int c=5;
      			if (code_seen('E')) e=code_value();
				if (e<0) temp=70;
      			if (code_seen('S')) temp=code_value();
      			if (code_seen('C')) c=code_value();
      			PID_autotune(temp, e, c);
    		}
    		break;
    		case 400: // M400 finish all moves
    		{
      			st_synchronize();
    		}
    		break;
    		case 500: // M500 Store settings in EEPROM
    		{
				Config_StoreSettings();
    		}
    		break;
    		case 501: // M501 Read settings from EEPROM
    		{
				Config_RetrieveSettings();
    		}
    		break;
    		case 502: // M502 Revert to default settings
    		{
				Config_ResetDefault();
    		}
    		break;
    		case 503: // M503 print settings currently in memory
    		{
				Config_PrintSettings();
    		}
    		break;
    		#ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
    		case 540:
    		{
				if(code_seen('S')) abort_on_endstop_hit = code_value() > 0;
    		}
    		break;
    		#endif
    		#ifdef FILAMENTCHANGEENABLE
			case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
			{
      			lcd_disable_display_timeout();
      
      			float target[4];
      			float lastpos[4];

      			target[X_AXIS]=current_position[X_AXIS];
      			target[Y_AXIS]=current_position[Y_AXIS];
      			target[Z_AXIS]=current_position[Z_AXIS];
      			target[E_AXIS]=current_position[E_AXIS];

      			lastpos[X_AXIS]=current_position[X_AXIS];
      			lastpos[Y_AXIS]=current_position[Y_AXIS];
      			lastpos[Z_AXIS]=current_position[Z_AXIS];
      			lastpos[E_AXIS]=current_position[E_AXIS];

      			if (code_seen('E')) {
        			target[E_AXIS] += code_value();
      			} else {
        			#ifdef FILAMENTCHANGE_FIRSTRETRACT
        				target[E_AXIS] += FILAMENTCHANGE_FIRSTRETRACT;
       				#endif // FILAMENTCHANGE_FIRSTRETRACT
      			}
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);

      			if (code_seen('Z')) {
        			target[Z_AXIS] += code_value();
      			} else {
        			#ifdef FILAMENTCHANGE_ZADD
        				target[Z_AXIS] += FILAMENTCHANGE_ZADD;
        			#endif // FILAMENTCHANGE_ZADD
      			}
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);

      			if (code_seen('X')) {
        			target[X_AXIS] = code_value();
      			} else {
        			#ifdef FILAMENTCHANGE_XPOS
        				target[X_AXIS] = FILAMENTCHANGE_XPOS;
        			#endif // FILAMENTCHANGE_XPOS
      			}
      			if(code_seen('Y')) {
        			target[Y_AXIS] = code_value();
      			} else {
        			#ifdef FILAMENTCHANGE_YPOS
        				target[Y_AXIS] = FILAMENTCHANGE_YPOS;
        			#endif // FILAMENTCHANGE_YPOS    
      			}
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feedrate/60, active_extruder);
      			st_synchronize();
				enable_x();
				enable_y();

      			lcd_enable_button();
      			draw_wizard_change_filament();
      			lcd_update(true);
      			SERIAL_ECHOLN("Wizard set to 0");

      			lcd_clear_triggered_flags();
      			while (!LCD_CLICKED){
        			manage_heater();
      			}

      			lcd_wizard_set_page(1);
      			lcd_update(true);
      			SERIAL_ECHOLN("Wizard set to 1");

      			target[E_AXIS] += 10.0;
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 300/60, active_extruder);
      			st_synchronize();
				enable_x();
				enable_y();

      			if (code_seen('L')) {
        			target[E_AXIS] += code_value();
      			} else {
        			#ifdef FILAMENTCHANGE_FINALRETRACT
        				target[E_AXIS]+= FILAMENTCHANGE_FINALRETRACT;
        			#endif // FILAMENTCHANGE_FINALRETRACT
      			}
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 300/60, active_extruder);
      			st_synchronize();
				enable_x();
				enable_y();

      			lcd_wizard_set_page(2);
      			lcd_update(true);
      			SERIAL_ECHOLN("Wizard set to 2");

      			disable_e0();
      			disable_e1();
      			disable_e2();

      			lcd_clear_triggered_flags();
      			while (!LCD_CLICKED){
       				manage_heater();
      			}

      			lcd_wizard_set_page(3);
      			lcd_update(true);
      			SERIAL_ECHOLN("Wizard set to 3");

      			lcd_clear_triggered_flags();
      			while (!LCD_CLICKED) {
        			manage_heater();
        			current_position[E_AXIS]+=0.04;
        			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],current_position[E_AXIS], feedrate/60, active_extruder);
        			st_synchronize();
					enable_x();
					enable_y();
      			} 

      			lcd_disable_button();

      			current_position[E_AXIS]=lastpos[E_AXIS];
      			plan_set_e_position(current_position[E_AXIS]);
    
      			plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //should do nothing
      			plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], target[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //move xy back
      			plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //move z back
      			plan_buffer_line(lastpos[X_AXIS], lastpos[Y_AXIS], lastpos[Z_AXIS], lastpos[E_AXIS], feedrate/60, active_extruder); //final untretract

      			lcd_wizard_set_page(4);
      			lcd_update(true);
      			lcd_enable_display_timeout();
      			SERIAL_ECHOLN("Wizard set to 4");

      			st_synchronize();
      			lcd_enable_button();
      			stop_buffer = false;
    		}
    		break;
    		#endif //FILAMENTCHANGEENABLE

    		#ifdef DUAL_X_CARRIAGE
    		case 605: // Set dual x-carriage movement mode:
	      	//    M605 S0: Full control mode. The slicer has full control over x-carriage movement
	      	//    M605 S1: Auto-park mode. The inactive head will auto park/unpark without slicer involvement
	      	//    M605 S2 [Xnnn] [Rmmm]: Duplication mode. The second extruder will duplicate the first with nnn
	      	//                         millimeters x-offset and an optional differential hotend temperature of
	      	//                         mmm degrees. E.g., with "M605 S2 X100 R2" the second extruder will duplicate
	      	//                         the first with a spacing of 100mm in the x direction and 2 degrees hotter.
	      	//
	      	//    Note: the X axis should be homed after changing dual x-carriage mode.
    		{
				st_synchronize();

				if (code_seen('S'))
	  				dual_x_carriage_mode = code_value();

				if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
				{
	  				if (code_seen('X'))
	    				duplicate_extruder_x_offset = max(code_value(),X2_MIN_POS - x_home_pos(0));
	  				if (code_seen('R'))
	    			duplicate_extruder_temp_offset = code_value();

	  				SERIAL_ECHO_START;
	  				SERIAL_ECHOPGM(MSG_HOTEND_OFFSET);
	  				SERIAL_ECHO(" ");
	  				SERIAL_ECHO(extruder_offset[X_AXIS][0]);
	  				SERIAL_ECHO(",");
	  				SERIAL_ECHO(extruder_offset[Y_AXIS][0]);
	  				SERIAL_ECHO(" ");
	  				SERIAL_ECHO(duplicate_extruder_x_offset);
	  				SERIAL_ECHO(",");
	  				SERIAL_ECHOLN(extruder_offset[Y_AXIS][1]);
				}
				else if (dual_x_carriage_mode != DXC_FULL_CONTROL_MODE && dual_x_carriage_mode != DXC_AUTO_PARK_MODE)
				{
	  				dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE;
				}

				active_extruder_parked = false;
				extruder_duplication_enabled = false;
				delayed_move_time = 0;
    		}
    		break;
    		#endif //DUAL_X_CARRIAGE

			#ifdef WITBOX 
    		/* WITBOX new GCODES:
			*M700: level plate script
			*M701: load filament script
			*M702: unload filament script */
    		case 700: // Script for level the build plate going to 3 points
    		{
  				SERIAL_ECHOLN(" --LEVEL PLATE SCRIPT--");
    			lcd_disable_display_timeout();
    			lcd_update();

    			lcd_clear_triggered_flags();
  				while(!LCD_CLICKED) {
      				manage_heater();
    			}
  	
  				lcd_wizard_set_page(1);
    			lcd_update();
         
    			saved_feedrate = feedrate;
    			saved_feedmultiply = feedmultiply;
    			feedmultiply = 100;
    			previous_millis_cmd = millis();

    			lcd_disable_interrupt();
    			enable_endstops(true);

    			for(int8_t i=0; i < NUM_AXIS; i++) {
    				destination[i] = current_position[i];
    			}
    			feedrate = 0.0;
    			home_all_axis = !((code_seen(axis_codes[0])) || (code_seen(axis_codes[1])) || (code_seen(axis_codes[2])));

  				#if Z_HOME_DIR > 0                      // If homing away from BED do Z first
    				if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
    					HOMEAXIS(Z);
    				}
  				#endif
    			if((home_all_axis) || (code_seen(axis_codes[X_AXIS]))) {
      				HOMEAXIS(X);
    			}
    			if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) {
      				HOMEAXIS(Y);
    			}
  				#if Z_HOME_DIR < 0                      // If homing towards BED do Z last
    				if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) {
    					HOMEAXIS(Z);
    				}
  				#endif

    			if(code_seen(axis_codes[X_AXIS])) {
      				if(code_value_long() != 0) {
        				current_position[X_AXIS]=code_value()+add_homeing[0];
      				}
   				}

    			if(code_seen(axis_codes[Y_AXIS])) {
      				if(code_value_long() != 0) {
        				current_position[Y_AXIS]=code_value()+add_homeing[1];
      				}
    			}

    			if(code_seen(axis_codes[Z_AXIS])) {
      				if(code_value_long() != 0) {
        				current_position[Z_AXIS]=code_value()+add_homeing[2];
      				}
    			}

    			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

  				#ifdef ENDSTOPS_ONLY_FOR_HOMING
  					enable_endstops(false);
  				#endif

    			feedrate = saved_feedrate;
    			feedmultiply = saved_feedmultiply;
    			previous_millis_cmd = millis();
    			endstops_hit_on_purpose();        
  	
    			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);

  				// prob 1
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+10);
    			lcd_enable_interrupt();
    
  				#if X_MAX_POS > 250 //Witbox
    				do_blocking_move_to((X_MAX_POS-X_MIN_POS)/2,Y_MAX_POS-10, current_position[Z_AXIS]);
  				#elif Y_MAX_POS > 250 //Hephestos XL
    				do_blocking_move_to(20, 260, current_position[Z_AXIS]);
  				#else //Hephestos
    				do_blocking_move_to(20, 190, current_position[Z_AXIS]);
  				#endif
    			do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_MIN_POS);
  	

    			lcd_clear_triggered_flags();
    			while(!LCD_CLICKED) {          
      				manage_heater();
    			}
  	
  				lcd_wizard_set_page(2);
    			lcd_update();
  	
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+10);
  				#if X_MAX_POS > 250 //Witbox
  					do_blocking_move_to(90, 5, current_position[Z_AXIS]);
				#elif Y_MAX_POS > 250 //Hephestos XL
					do_blocking_move_to(190, 260, current_position[Z_AXIS]);
  				#else //Hephestos
  					do_blocking_move_to(195, 190, current_position[Z_AXIS]);
  				#endif
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS);
  	  
    			lcd_clear_triggered_flags();
  				while(!LCD_CLICKED) {
  	  				manage_heater();
  	  				manage_inactivity();
  				}
  	
  				lcd_wizard_set_page(3);
    			lcd_update();
  		  
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+10);
  				#if X_MAX_POS > 250 //Witbox
    				do_blocking_move_to(205, 5, current_position[Z_AXIS]);
				#elif Y_MAX_POS > 250 //Hephestos XL
					do_blocking_move_to(20, 40, current_position[Z_AXIS]);
  				#else //Hephestos
    				do_blocking_move_to(20, 20, current_position[Z_AXIS]);
  				#endif
  					do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS);

    			lcd_clear_triggered_flags();
  	 			while(!LCD_CLICKED) {
  	  				manage_heater();
  	  				manage_inactivity();
  				}

    			#if X_MAX_POS < 250
    				lcd_wizard_set_page(4);
    				lcd_update();
  		  
    				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+10);
    				#if Y_MAX_POS > 250 //Hephestos XL
    					do_blocking_move_to(190, 40, current_position[Z_AXIS]);
    				#else //Hephestos
    					do_blocking_move_to(195, 20, current_position[Z_AXIS]);
    				#endif
    				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS);
  	
    				lcd_clear_triggered_flags();
    				while(!LCD_CLICKED){
      					manage_heater();
      					manage_inactivity();
    				}
    			#endif

    			lcd_wizard_set_page(5);
    			lcd_update();
  		 
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+10);
  				#if X_MAX_POS > 250
  	  				do_blocking_move_to(150, 105, current_position[Z_AXIS]);
  				#else
    				do_blocking_move_to((X_MAX_POS-X_MIN_POS)/2, (Y_MAX_POS-Y_MIN_POS)/2, current_position[Z_AXIS]);
  				#endif
  					do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS);
  	      
    			lcd_clear_triggered_flags();
  				while(!LCD_CLICKED){                  
  	  				manage_heater();
  	  				manage_inactivity();
  				}
  	
  				lcd_wizard_set_page(6);
    			lcd_update();
  		
  				do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS+50);
  				do_blocking_move_to(10, 10, current_position[Z_AXIS]);
  				//do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS],Z_MIN_POS);
    			lcd_wizard_set_page(7);
    			lcd_update();      
    			lcd_enable_display_timeout();
    		}
    		break;
    
    		case 701:
      			SERIAL_ECHOLN(" --LOAD-- ");
      
       			st_synchronize();
       			plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); 
    
      			//-- Extruir!
      			current_position[E_AXIS] += 100.0;
      			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],current_position[E_AXIS], 300/60, active_extruder);
      			st_synchronize(); 

      			SERIAL_ECHOLN("ok");
				//do_blocking_extrude_to(100);
      			break;
			case 702:
				SERIAL_ECHOLN(" --UNLOAD-- ");
      
				st_synchronize(); 
				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); 
    
				//-- Extruir!
				current_position[E_AXIS] += 50.0;
				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],current_position[E_AXIS], 300/60, active_extruder);
				st_synchronize(); 
      
				//-- Sacar!
				current_position[E_AXIS] -= 60.0;
				plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],current_position[E_AXIS], 300/60, active_extruder);
				st_synchronize();

				SERIAL_ECHOLN("ok");
				//do_blocking_extrude_to(30);
				//do_blocking_extrude_to(-100);
      			break;  
			#endif //WITBOX

    		case 907: // M907 Set digital trimpot motor current using axis codes.
    		{
      			#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
					for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_current(i,code_value());
					if(code_seen('B')) digipot_current(4,code_value());
					if(code_seen('S')) for(int i=0;i<=4;i++) digipot_current(i,code_value());
      			#endif
      			#ifdef MOTOR_CURRENT_PWM_XY_PIN
					if(code_seen('X')) digipot_current(0, code_value());
      			#endif
      			#ifdef MOTOR_CURRENT_PWM_Z_PIN
					if(code_seen('Z')) digipot_current(1, code_value());
      			#endif
      			#ifdef MOTOR_CURRENT_PWM_E_PIN
					if(code_seen('E')) digipot_current(2, code_value());
     			#endif
      			#ifdef DIGIPOT_I2C
					// this one uses actual amps in floating point
					for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value());
					// for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
					for(int i=NUM_AXIS;i<DIGIPOT_I2C_NUM_CHANNELS;i++) if(code_seen('B'+i-NUM_AXIS)) digipot_i2c_set_current(i, code_value());
      			#endif
    		}
    		break;
    		case 908: // M908 Control digital trimpot directly.
    		{
      			#if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
					uint8_t channel,current;
					if(code_seen('P')) channel=code_value();
					if(code_seen('S')) current=code_value();
					digitalPotWrite(channel, current);
      			#endif
    		}
    		break;
    		case 350: // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
    		{
				#if defined(X_MS1_PIN) && X_MS1_PIN > -1
					if(code_seen('S')) for(int i=0;i<=4;i++) microstep_mode(i,code_value());
					for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_mode(i,(uint8_t)code_value());
					if(code_seen('B')) microstep_mode(4,code_value());
					microstep_readings();
				#endif
			}
			break;
    		case 351: // M351 Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
    		{
      			#if defined(X_MS1_PIN) && X_MS1_PIN > -1
      				if(code_seen('S')) switch((int)code_value())
      				{
						case 1:
	  						for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,code_value(),-1);
	  						if(code_seen('B')) microstep_ms(4,code_value(),-1);
	  						break;
						case 2:
	  						for(int i=0;i<NUM_AXIS;i++) if(code_seen(axis_codes[i])) microstep_ms(i,-1,code_value());
	  						if(code_seen('B')) microstep_ms(4,-1,code_value());
	  						break;
      				}
      				microstep_readings();
      			#endif
    		}
    		break;
    		case 999: // M999: Restart after being stopped
      			Stopped = false;
      			lcd_reset_alert_level();
      			gcode_LastN = Stopped_gcode_LastN;
      			FlushSerialRequestResend();
    		break;
    	}
	}
  	else if(code_seen('T'))
  	{
		tmp_extruder = code_value();
		if(tmp_extruder >= EXTRUDERS) {
			SERIAL_ECHO_START;
			SERIAL_ECHO("T");
			SERIAL_ECHO(tmp_extruder);
			SERIAL_ECHOLN(MSG_INVALID_EXTRUDER);
		}
		else {
			boolean make_move = false;
			if(code_seen('F')) {
				make_move = true;
				next_feedrate = code_value();
				if(next_feedrate > 0.0) {
					feedrate = next_feedrate;
				}
			}
      		#if EXTRUDERS > 1
      		if(tmp_extruder != active_extruder) {
				// Save current position to return to after applying extruder offset
				memcpy(destination, current_position, sizeof(destination));
      			#ifdef DUAL_X_CARRIAGE
					if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE && Stopped == false && (delayed_move_time != 0 || current_position[X_AXIS] != x_home_pos(active_extruder)))
					{
	  					// Park old head: 1) raise 2) move to park position 3) lower
	  					plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT, current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
	  					plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS] + TOOLCHANGE_PARK_ZLIFT, current_position[E_AXIS], max_feedrate[X_AXIS], active_extruder);
	  					plan_buffer_line(x_home_pos(active_extruder), current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
	  					st_synchronize();
					}

					// apply Y & Z extruder offset (x offset is already used in determining home pos)
					current_position[Y_AXIS] = current_position[Y_AXIS] - extruder_offset[Y_AXIS][active_extruder] + extruder_offset[Y_AXIS][tmp_extruder];
					current_position[Z_AXIS] = current_position[Z_AXIS] - extruder_offset[Z_AXIS][active_extruder] + extruder_offset[Z_AXIS][tmp_extruder];

					active_extruder = tmp_extruder;

					// This function resets the max/min values - the current position may be overwritten below.
					axis_is_at_home(X_AXIS);

					if (dual_x_carriage_mode == DXC_FULL_CONTROL_MODE)
					{
	  					current_position[X_AXIS] = inactive_extruder_x_pos;
	  					inactive_extruder_x_pos = destination[X_AXIS];
					}
					else if (dual_x_carriage_mode == DXC_DUPLICATION_MODE)
					{
	  					active_extruder_parked = (active_extruder == 0); // this triggers the second extruder to move into the duplication position
	  					if (active_extruder == 0 || active_extruder_parked)
	    					current_position[X_AXIS] = inactive_extruder_x_pos;
	  					else
	    					current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset;
	  					inactive_extruder_x_pos = destination[X_AXIS];
	  					extruder_duplication_enabled = false;
					}
					else
					{
	  					// record raised toolhead position for use by unpark
	  					memcpy(raised_parked_position, current_position, sizeof(raised_parked_position));
	  					raised_parked_position[Z_AXIS] += TOOLCHANGE_UNPARK_ZLIFT;
	  					active_extruder_parked = true;
	  					delayed_move_time = 0;
					}
      			#else
					// Offset extruder (only by XY)
					int i;
					for(i = 0; i < 2; i++) {
	   					current_position[i] = current_position[i] - extruder_offset[i][active_extruder] + extruder_offset[i][tmp_extruder];
					}
					// Set the new active extruder and position
					active_extruder = tmp_extruder;
      			#endif //else DUAL_X_CARRIAGE
				plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
				// Move to the old position if 'F' was in the parameters
				if(make_move && Stopped == false) {
	   				prepare_move();
				}
      		}
      		#endif
      		SERIAL_ECHO_START;
      		SERIAL_ECHO(MSG_ACTIVE_EXTRUDER);
      		SERIAL_PROTOCOLLN((int)active_extruder);
    	}
	}
	else
	{
		SERIAL_ECHO_START;
		SERIAL_ECHOPGM(MSG_UNKNOWN_COMMAND);
		SERIAL_ECHO(cmdbuffer[bufindr]);
		SERIAL_ECHOLNPGM("\"");
	}
	ClearToSend();
}

void FlushSerialRequestResend()
{
	//char cmdbuffer[bufindr][100]="Resend:";
	MYSERIAL.flush();
	SERIAL_PROTOCOLPGM(MSG_RESEND);
	SERIAL_PROTOCOLLN(gcode_LastN + 1);
	ClearToSend();
}

void ClearToSend()
{
	previous_millis_cmd = millis();
	#ifdef SDSUPPORT
		if(fromsd[bufindr])
			return;
	#endif //SDSUPPORT
	SERIAL_PROTOCOLLNPGM(MSG_OK);
}

void get_coordinates()
{
	bool seen[4]={false,false,false,false};
	for(int8_t i=0; i < NUM_AXIS; i++) {
		if(code_seen(axis_codes[i]))
		{
			destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
			seen[i]=true;
		}
		else destination[i] = current_position[i]; //Are these else lines really needed?
		}
		if(code_seen('F')) {
			next_feedrate = code_value();
			if(next_feedrate > 0.0) feedrate = next_feedrate;
		}
}

void get_arc_coordinates()
{
	#ifdef SF_ARC_FIX
		bool relative_mode_backup = relative_mode;
		relative_mode = true;
	#endif
	get_coordinates();
	#ifdef SF_ARC_FIX
		relative_mode=relative_mode_backup;
	#endif

	if(code_seen('I')) {
		offset[0] = code_value();
	}
	else {
		offset[0] = 0.0;
	}
	if(code_seen('J')) {
		offset[1] = code_value();
	}
	else {
		offset[1] = 0.0;
	}
}

void clamp_to_software_endstops(float target[3])
{
	if (min_software_endstops) {
		if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
		if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
		if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
	}

	if (max_software_endstops) {
		if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
		if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
		if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
	}
}

void prepare_move()
{
	clamp_to_software_endstops(destination);

	previous_millis_cmd = millis();

	#ifdef DUAL_X_CARRIAGE
	if (active_extruder_parked)
  	{
    	if (dual_x_carriage_mode == DXC_DUPLICATION_MODE && active_extruder == 0)
    	{
      		// move duplicate extruder into correct duplication position.
      		plan_set_position(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
      		plan_buffer_line(current_position[X_AXIS] + duplicate_extruder_x_offset, current_position[Y_AXIS], current_position[Z_AXIS],
	  		current_position[E_AXIS], max_feedrate[X_AXIS], 1);
      		plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
      		st_synchronize();
      		extruder_duplication_enabled = true;
      		active_extruder_parked = false;
    	}
    	else if (dual_x_carriage_mode == DXC_AUTO_PARK_MODE) // handle unparking of head
    	{
      		if (current_position[E_AXIS] == destination[E_AXIS])
      		{
				// this is a travel move - skit it but keep track of current position (so that it can later
				// be used as start of first non-travel move)
				if (delayed_move_time != 0xFFFFFFFFUL)
				{
	  				memcpy(current_position, destination, sizeof(current_position));
	  				if (destination[Z_AXIS] > raised_parked_position[Z_AXIS])
	    			raised_parked_position[Z_AXIS] = destination[Z_AXIS];
	  				delayed_move_time = millis();
	  				return;
				}
      		}
      		delayed_move_time = 0;
      		// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
      		plan_buffer_line(raised_parked_position[X_AXIS], raised_parked_position[Y_AXIS], raised_parked_position[Z_AXIS],    current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
      		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], raised_parked_position[Z_AXIS],
	  		current_position[E_AXIS], min(max_feedrate[X_AXIS],max_feedrate[Y_AXIS]), active_extruder);
      		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
	  		current_position[E_AXIS], max_feedrate[Z_AXIS], active_extruder);
      		active_extruder_parked = false;
    	}
	}
	#endif //DUAL_X_CARRIAGE

	// Do not use feedmultiply for E or Z only moves
	if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
		plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
	}
	else {
		plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
	}
	for(int8_t i=0; i < NUM_AXIS; i++) {
		current_position[i] = destination[i];
	}
}

void prepare_arc_move(char isclockwise) {
	float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc

	// Trace the arc
	mc_arc(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60/100.0, r, isclockwise, active_extruder);

	// As far as the parser is concerned, the position is now == target. In reality the
	// motion control system might still be processing the action and the real tool position
	// in any intermediate location.
	for(int8_t i=0; i < NUM_AXIS; i++) {
		current_position[i] = destination[i];
	}
	previous_millis_cmd = millis();
}

#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1

#if defined(FAN_PIN)
	#if CONTROLLERFAN_PIN == FAN_PIN
		#error "You cannot set CONTROLLERFAN_PIN equal to FAN_PIN"
	#endif
#endif

unsigned long lastMotor = 0; //Save the time for when a motor was turned on last
unsigned long lastMotorCheck = 0;

void controllerFan()
{
	if ((millis() - lastMotorCheck) >= 2500) //Not a time critical function, so we only check every 2500ms
	{
		lastMotorCheck = millis();

		if(!READ(X_ENABLE_PIN) || !READ(Y_ENABLE_PIN) || !READ(Z_ENABLE_PIN) || (soft_pwm_bed > 0)
    	#if EXTRUDERS > 2
			|| !READ(E2_ENABLE_PIN)
		#endif
		#if EXTRUDER > 1
			#if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
       			|| !READ(X2_ENABLE_PIN)
      		#endif
       		|| !READ(E1_ENABLE_PIN)
    	#endif
		|| !READ(E0_ENABLE_PIN)) //If any of the drivers are enabled...
		{
			lastMotor = millis(); //... set time to NOW so the fan will turn on
		}

		if ((millis() - lastMotor) >= (CONTROLLERFAN_SECS*1000UL) || lastMotor == 0) //If the last time any driver was enabled, is longer since than CONTROLLERSEC...
		{
			digitalWrite(CONTROLLERFAN_PIN, 0);
			analogWrite(CONTROLLERFAN_PIN, 0);
		}
		else
		{
			// allows digital or PWM fan output to be used (see M42 handling)
			digitalWrite(CONTROLLERFAN_PIN, CONTROLLERFAN_SPEED);
			analogWrite(CONTROLLERFAN_PIN, CONTROLLERFAN_SPEED);
		}
	}
}
#endif

void manage_inactivity()
{
	if( (millis() - previous_millis_cmd) >  max_inactive_time )
		if(max_inactive_time)
			kill();
	if(stepper_inactive_time)  {
    	if( (millis() - previous_millis_cmd) >  stepper_inactive_time )
		{
			if(blocks_queued() == false) {
				disable_x();
				disable_y();
				disable_z();
				disable_e0();
				disable_e1();
				disable_e2();
			}
		}
	}
	#if defined(KILL_PIN) && KILL_PIN > -1
		if( 0 == READ(KILL_PIN) )
			kill();
	#endif
	#if defined(CONTROLLERFAN_PIN) && CONTROLLERFAN_PIN > -1
		controllerFan(); //Check if fan should be turned on to cool stepper drivers down
	#endif
	#ifdef EXTRUDER_RUNOUT_PREVENT
		if( (millis() - previous_millis_cmd) >  EXTRUDER_RUNOUT_SECONDS*1000 )
		if(degHotend(active_extruder)>EXTRUDER_RUNOUT_MINTEMP)
		{
			bool oldstatus=READ(E0_ENABLE_PIN);
			enable_e0();
			float oldepos=current_position[E_AXIS];
			float oldedes=destination[E_AXIS];
			plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS]+EXTRUDER_RUNOUT_EXTRUDE*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS],
		      EXTRUDER_RUNOUT_SPEED/60.*EXTRUDER_RUNOUT_ESTEPS/axis_steps_per_unit[E_AXIS], active_extruder);
			current_position[E_AXIS]=oldepos;
			destination[E_AXIS]=oldedes;
			plan_set_e_position(oldepos);
			previous_millis_cmd=millis();
			st_synchronize();
			WRITE(E0_ENABLE_PIN,oldstatus);
		}
	#endif
	#if defined(DUAL_X_CARRIAGE)
    	// handle delayed move timeout
    	if (delayed_move_time != 0 && (millis() - delayed_move_time) > 1000 && Stopped == false)
    	{
      		// travel moves have been received so enact them
      		delayed_move_time = 0xFFFFFFFFUL; // force moves to be done
      		memcpy(destination,current_position,sizeof(destination));
      		prepare_move();
    	}
  	#endif
	check_axes_activity();
}

void kill()
{
	cli(); // Stop interrupts
	disable_heater();

	disable_x();
	disable_y();
	disable_z();
	disable_e0();
	disable_e1();
	disable_e2();

	#if defined(PS_ON_PIN) && PS_ON_PIN > -1
		pinMode(PS_ON_PIN,INPUT);
	#endif
	SERIAL_ERROR_START;
	SERIAL_ERRORLNPGM(MSG_ERR_KILLED);
	LCD_ALERTMESSAGEPGM(MSG_KILLED);
	suicide();
	while(1) { /* Intentionally left empty */ } // Wait for reset
}

void Stop()
{
	disable_heater();
	if(Stopped == false) {
	Stopped = true;
	Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
	SERIAL_ERROR_START;
	SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
	LCD_MESSAGEPGM(MSG_STOPPED);
	}
}

bool IsStopped() { return Stopped; };

bool setTargetedHotend(int code)
{
	tmp_extruder = active_extruder;
	if(code_seen('T')) {
		tmp_extruder = code_value();
		if(tmp_extruder >= EXTRUDERS) {
			SERIAL_ECHO_START;
			switch(code){
				case 104:
					SERIAL_ECHO(MSG_M104_INVALID_EXTRUDER);
					break;
				case 105:
					SERIAL_ECHO(MSG_M105_INVALID_EXTRUDER);
					break;
				case 109:
					SERIAL_ECHO(MSG_M109_INVALID_EXTRUDER);
					break;
				case 218:
					SERIAL_ECHO(MSG_M218_INVALID_EXTRUDER);
					break;
			}
      		SERIAL_ECHOLN(tmp_extruder);
      		return true;
		}
	}
	return false;
}
