- Implement Plannercode from Marlin V1 --> big thanks to Erik

  - Stepper interrupt with Step loops
  - Stepperfrequenz 30 Khz
  - New Command
    * M202 - 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) im mm/sec^2
    * M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk
  - Remove unused Variables
  - Check Uart Puffer while circle processing (CMD: G2 / G3)
  - Fast Xfer Function --> move Text to Flash
  - Option to deaktivate ARC (G2/G3) function (save flash)
  - Removed modulo (%) operator, which uses an expensive divide

3 files changed, 905 insertions, 495 deletions

diff --git a/Sprinter/Configuration.h b/Sprinter/Configuration.h
index 25b3be6..86b7115 100644
--- a/Sprinter/Configuration.h
+++ b/Sprinter/Configuration.h
@@ -56,8 +56,13 @@ const bool Z_ENDSTOP_INVERT = false;
 // Uncomment to make run init.g from SD on boot
 //#define SDINITFILE
 
+//Only work with Atmega1284 you need +1 kb ram
 //#define SD_FAST_XFER_AKTIV
 
+//Uncomment to aktivate the arc (circle) function (G2/G3 Command)
+//Without SD function an ARC function the used Flash is smaller 31 kb
+#define USE_ARC_FUNCTION
+
 //-----------------------------------------------------------------------
 //// ADVANCED SETTINGS - to tweak parameters
 //-----------------------------------------------------------------------
@@ -69,8 +74,9 @@ const bool Z_ENDSTOP_INVERT = false;
 	#endif
 #endif
 
-
+//-----------------------------------------------------------------------
 // For Inverting Stepper Enable Pins (Active Low) use 0, Non Inverting (Active High) use 1
+//-----------------------------------------------------------------------
 #define X_ENABLE_ON 0
 #define Y_ENABLE_ON 0
 #define Z_ENABLE_ON 0
@@ -79,40 +85,57 @@ const bool Z_ENDSTOP_INVERT = false;
 //Uncomment if you have problems with a stepper driver enabeling too late, this will also set how many microseconds delay there will be after enabeling the driver
 //#define DELAY_ENABLE 15
 
+//-----------------------------------------------------------------------
 // Disables axis when it's not being used.
+//-----------------------------------------------------------------------
 const bool DISABLE_X = false;
 const bool DISABLE_Y = false;
 const bool DISABLE_Z = true;
 const bool DISABLE_E = false;
 
+//-----------------------------------------------------------------------
 // Inverting axis direction
+//-----------------------------------------------------------------------
 const bool INVERT_X_DIR = false;
 const bool INVERT_Y_DIR = false;
 const bool INVERT_Z_DIR = true;
 const bool INVERT_E_DIR = false;
 
-
+//-----------------------------------------------------------------------
 //// ENDSTOP SETTINGS:
+//-----------------------------------------------------------------------
 // Sets direction of endstops when homing; 1=MAX, -1=MIN
 #define X_HOME_DIR -1
 #define Y_HOME_DIR -1
 #define Z_HOME_DIR -1
 
+//#define ENDSTOPS_ONLY_FOR_HOMING // If defined the endstops will only be used for homing
 
 const bool min_software_endstops = false; //If true, axis won't move to coordinates less than zero.
 const bool max_software_endstops = true; //If true, axis won't move to coordinates greater than the defined lengths below.
 
+
+//-----------------------------------------------------------------------
 //Max Length for Prusa Mendel, check the ways of your axis and set this Values
+//-----------------------------------------------------------------------
 const int X_MAX_LENGTH = 200;
 const int Y_MAX_LENGTH = 200;
 const int Z_MAX_LENGTH = 100;
 
+//-----------------------------------------------------------------------
 //// MOVEMENT SETTINGS
+//-----------------------------------------------------------------------
 const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z, E
-#define _MAX_FEEDRATE {200000, 200000, 240, 500000}
-#define _HOMING_FEEDRATE {1500,1500,120}
+#define _MAX_FEEDRATE {400, 400, 4, 45}       // (mm/sec)    
+#define _HOMING_FEEDRATE {1500,1500,120}      // (mm/min) !!
 #define _AXIS_RELATIVE_MODES {false, false, false, false}
 
+#define MAX_STEP_FREQUENCY 30000 // Max step frequency
+
+
+//-----------------------------------------------------------------------
+//// Not used at the Moment
+//-----------------------------------------------------------------------
 
 // Min step delay in microseconds. If you are experiencing missing steps, try to raise the delay microseconds, but be aware this
 // If you enable this, make sure STEP_DELAY_RATIO is disabled.
@@ -128,23 +151,35 @@ const int NUM_AXIS = 4; // The axis order in all axis related arrays is X, Y, Z,
 long min_time_before_dir_change = 30; //milliseconds
 #endif
 
-// Comment this to disable ramp acceleration
-#define RAMP_ACCELERATION
-
+//-----------------------------------------------------------------------
 //// Acceleration settings
-#ifdef RAMP_ACCELERATION
+//-----------------------------------------------------------------------
 // X, Y, Z, E maximum start speed for accelerated moves. E default values are good for skeinforge 40+, for older versions raise them a lot.
-#define _ACCELERATION 2000         // Normal acceleration mm/s^2
-#define _RETRACT_ACCELERATION 7000 // Normal acceleration mm/s^2
-#define _MAX_XY_JERK (20.0*60)
-#define _MAX_Z_JERK (0.4*60)
-#define _MAX_START_SPEED_UNITS_PER_SECOND {25.0,25.0,0.2,10.0}
-#define _MAX_ACCELERATION_UNITS_PER_SQ_SECOND {500,500,50,500} // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
-#define _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND {500,500,50,500} // X, Y, Z max acceleration in mm/s^2 for travel moves
-#endif
+#define _ACCELERATION 1000         // Axis Normal acceleration mm/s^2
+#define _RETRACT_ACCELERATION 2000 // Extruder Normal acceleration mm/s^2
+#define _MAX_XY_JERK 20.0
+#define _MAX_Z_JERK 0.4
+//#define _MAX_START_SPEED_UNITS_PER_SECOND {25.0,25.0,0.2,10.0}
+#define _MAX_ACCELERATION_UNITS_PER_SQ_SECOND {5000,5000,50,5000}    // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
 
 
+// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
+// of the buffer and all stops. This should not be much greater than zero and should only be changed
+// if unwanted behavior is observed on a user's machine when running at very slow speeds.
+#define MINIMUM_PLANNER_SPEED 2.0 // (mm/sec)
+
+#define DEFAULT_MINIMUMFEEDRATE       0.0     // minimum feedrate
+#define DEFAULT_MINTRAVELFEEDRATE     0.0
+
+// If defined the movements slow down when the look ahead buffer is only half full
+#define SLOWDOWN
+
+
+const int dropsegments=5; //everything with less than this number of steps will be ignored as move and joined with the next movement
+
+//-----------------------------------------------------------------------
 // Machine UUID
+//-----------------------------------------------------------------------
 // This may be useful if you have multiple machines and wish to identify them by using the M115 command. 
 // By default we set it to zeros.
 #define _DEF_CHAR_UUID "00000000-0000-0000-0000-000000000000"
@@ -290,7 +325,6 @@ long min_time_before_dir_change = 30; //milliseconds
 //#define DEBUG
 #ifdef DEBUG
   //#define DEBUG_PREPARE_MOVE //Enable this to debug prepare_move() function
-  //#define DEBUG_RAMP_ACCELERATION //Enable this to debug all constant acceleration info
   //#define DEBUG_MOVE_TIME //Enable this to time each move and print the result   
   //#define DEBUG_HEAT_MGMT //Enable this to debug heat management. WARNING, this will cause axes to jitter!
   //#define DEBUG_DISABLE_CHECK_DURING_TRAVEL //Debug the namesake feature, see above in this file
diff --git a/Sprinter/Sprinter.h b/Sprinter/Sprinter.h
index 844fc34..b70d1fd 100644
--- a/Sprinter/Sprinter.h
+++ b/Sprinter/Sprinter.h
@@ -1,10 +1,18 @@
 // Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware.
 // Licence: GPL
-#include <WProgram.h>
+
+//Check Version of Arduino and then include the right libraries
+#if defined(ARDUINO) && ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include <WProgram.h>  
+#endif
+
 #include "fastio.h"
 
 extern "C" void __cxa_pure_virtual();
 
+#define  FORCE_INLINE __attribute__((always_inline)) inline
 
 #if X_ENABLE_PIN > -1
 #define  enable_x() WRITE(X_ENABLE_PIN, X_ENABLE_ON)
@@ -47,10 +55,10 @@ typedef struct {
   // Fields used by the bresenham algorithm for tracing the line
   long steps_x, steps_y, steps_z, steps_e;  // Step count along each axis
 
-  long step_event_count;                    // The number of step events required to complete this block
-  volatile long accelerate_until;           // The index of the step event on which to stop acceleration
-  volatile long decelerate_after;           // The index of the step event on which to start decelerating
-  volatile long acceleration_rate;          // The acceleration rate used for acceleration calculation
+  unsigned long step_event_count;                    // The number of step events required to complete this block
+  long accelerate_until;           // The index of the step event on which to stop acceleration
+  long decelerate_after;           // The index of the step event on which to start decelerating
+  long acceleration_rate;          // The acceleration rate used for acceleration calculation
   unsigned char direction_bits;             // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
 
   #ifdef ADVANCE
@@ -61,16 +69,20 @@ typedef struct {
   #endif
 
   // Fields used by the motion planner to manage acceleration
-  float speed_x, speed_y, speed_z, speed_e;          // Nominal mm/minute for each axis
+//  float speed_x, speed_y, speed_z, speed_e;          // Nominal mm/minute for each axis
   float nominal_speed;                               // The nominal speed for this block in mm/min  
+  float entry_speed;                                 // Entry speed at previous-current junction in mm/min
+  float max_entry_speed;                             // Maximum allowable junction entry speed in mm/min
   float millimeters;                                 // The total travel of this block in mm
-  float entry_speed;
   float acceleration;                                // acceleration mm/sec^2
+  unsigned char recalculate_flag;                    // Planner flag to recalculate trapezoids on entry junction
+  unsigned char nominal_length_flag;                 // Planner flag for nominal speed always reached
+
 
   // Settings for the trapezoid generator
   long nominal_rate;                                 // The nominal step rate for this block in step_events/sec 
-  volatile long initial_rate;                        // The jerk-adjusted step rate at start of block  
-  volatile long final_rate;                          // The minimal rate at exit
+  long initial_rate;                        // The jerk-adjusted step rate at start of block  
+  long final_rate;                          // The minimal rate at exit
   long acceleration_st;                              // acceleration steps/sec^2
   volatile char busy;
 } block_t;
@@ -87,7 +99,6 @@ void manage_inactivity(byte debug);
 void get_coordinates();
 void prepare_move();
 void prepare_arc_move(char isclockwise);
-void plan_buffer_line(float x, float y, float z, float e, float feed_rate);
 
 void kill(byte debug);
 
diff --git a/Sprinter/Sprinter.pde b/Sprinter/Sprinter.pde
index 8cfe691..d33480f 100644
--- a/Sprinter/Sprinter.pde
+++ b/Sprinter/Sprinter.pde
@@ -1,6 +1,6 @@
 /*
  Reprap firmware based on Sprinter
- Optimize for Sanguinololu 1.2 and above
+ Optimize for Sanguinololu 1.2 and above, RAMPS 
  
  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
@@ -27,8 +27,7 @@
   Parts of Marlin Firmware from ErikZalm
   https://github.com/ErikZalm/Marlin-non-gen6
   
-  Sprinter V2
-
+  Sprinter Changelog
   -  Look forward function --> calculate 16 Steps forward, get from Firmaware Marlin and Grbl
   -  Stepper control with Timer 1 (Interrupt)
   -  Extruder heating with PID use a Softpwm (Timer 2) with 500 hz to free Timer1 für Steppercontrol
@@ -40,7 +39,19 @@
   -  move string to flash to free RAM vor forward planner
   -  M203 Temperature monitor for Repetier
 
-
+  
+  - Implement Plannercode from Marlin V1 big thanks to Erik
+  - Stepper interrupt with Step loops
+  - Stepperfrequenz 30 Khz
+  - New Command
+    * M202 - 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) im mm/sec^2 
+    * M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk
+  - Remove unused Variables
+  - Check Uart Puffer while circle processing (CMD: G2 / G3)
+  - Fast Xfer Function --> move Text to Flash
+  - Option to deaktivate ARC (G2/G3) function (save flash)
+  - Removed modulo (%) operator, which uses an expensive divide
 
 */
 
@@ -52,7 +63,9 @@
 #include "pins.h"
 #include "Sprinter.h"
 #include "speed_lookuptable.h"
-#include "arc_func.h"
+#ifdef USE_ARC_FUNCTION
+  #include "arc_func.h"
+#endif
 #include "heater.h"
 
 #ifdef SDSUPPORT
@@ -115,9 +128,12 @@ void __cxa_pure_virtual(){};
 // M119 - Show Endstopper State 
 // M140 - Set bed target temp
 // M190 - Wait for bed current temp to reach target temp.
-// 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)
+// M201 - Set maximum acceleration in units/s^2 for print moves (M201 X1000 Y1000)
+// M202 - Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in mm/sec
 // M203 - Set temperture monitor to Sx
+// M204 - Set default acceleration: S normal moves T filament only moves (M204 S3000 T7000) im mm/sec^2 
+// M205 - advanced settings:  minimum travel speed S=while printing T=travel only,  X= maximum xy jerk, Z=maximum Z jerk
+
 // M220 - set speed factor override percentage S:factor in percent 
 
 // Debug feature / Testing the PID for Hotend
@@ -126,7 +142,7 @@ void __cxa_pure_virtual(){};
 // M603 - Show Free Ram
 
 
-#define _VERSION_TEXT "1.2.20T / 27.01.2012"
+#define _VERSION_TEXT "1.3.04T / 04.02.2012"
 
 //Stepper Movement Variables
 char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
@@ -136,52 +152,61 @@ float max_feedrate[] = _MAX_FEEDRATE;
 float homing_feedrate[] = _HOMING_FEEDRATE;
 bool axis_relative_modes[] = _AXIS_RELATIVE_MODES;
 
-bool move_direction[NUM_AXIS];
-unsigned long axis_previous_micros[NUM_AXIS];
-unsigned long previous_micros = 0;
-unsigned long move_steps_to_take[NUM_AXIS];
-
-#ifdef RAMP_ACCELERATION
-  float acceleration = _ACCELERATION;         // Normal acceleration mm/s^2
-  float retract_acceleration = _RETRACT_ACCELERATION; // Normal acceleration mm/s^2
-  float max_xy_jerk = _MAX_XY_JERK;
-  float max_z_jerk = _MAX_Z_JERK;
-  float max_start_speed_units_per_second[] = _MAX_START_SPEED_UNITS_PER_SECOND;
-  long  max_acceleration_units_per_sq_second[] = _MAX_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
-  long  max_travel_acceleration_units_per_sq_second[] = _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z max acceleration in mm/s^2 for travel moves
-
-  unsigned long axis_max_interval[NUM_AXIS];
-  unsigned long axis_steps_per_sqr_second[NUM_AXIS];
-  unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS];
-  unsigned long max_interval;
-  unsigned long steps_per_sqr_second, plateau_steps;  
-#endif
+float move_acceleration = _ACCELERATION;         // Normal acceleration mm/s^2
+float retract_acceleration = _RETRACT_ACCELERATION; // Normal acceleration mm/s^2
+float max_xy_jerk = _MAX_XY_JERK;
+float max_z_jerk = _MAX_Z_JERK;
+
+long  max_acceleration_units_per_sq_second[] = _MAX_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z and E max acceleration in mm/s^2 for printing moves or retracts
+
+//float max_start_speed_units_per_second[] = _MAX_START_SPEED_UNITS_PER_SECOND;
+//long  max_travel_acceleration_units_per_sq_second[] = _MAX_TRAVEL_ACCELERATION_UNITS_PER_SQ_SECOND; // X, Y, Z max acceleration in mm/s^2 for travel moves
+
+float mintravelfeedrate = DEFAULT_MINTRAVELFEEDRATE;
+float minimumfeedrate = DEFAULT_MINIMUMFEEDRATE;
+
+unsigned long axis_steps_per_sqr_second[NUM_AXIS];
+unsigned long plateau_steps;  
+
+//unsigned long axis_max_interval[NUM_AXIS];
+//unsigned long axis_travel_steps_per_sqr_second[NUM_AXIS];
+//unsigned long max_interval;
+//unsigned long steps_per_sqr_second;
+
 
 //adjustable feed faktor for online tuning printerspeed
 volatile int feedmultiply=100; //100->original / 200-> Faktor 2 / 50 -> Faktor 0.5
 int saved_feedmultiply;
 volatile bool feedmultiplychanged=false;
 
-boolean acceleration_enabled = false, accelerating = false;
-unsigned long interval;
+//boolean acceleration_enabled = false, accelerating = false;
+//unsigned long interval;
 float destination[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
 float current_position[NUM_AXIS] = {0.0, 0.0, 0.0, 0.0};
-unsigned long steps_taken[NUM_AXIS];
-long axis_interval[NUM_AXIS]; // for speed delay
+
+
 bool home_all_axis = true;
+//unsigned ?? ToDo: Check
 int feedrate = 1500, next_feedrate, saved_feedrate;
-float time_for_move;
+
 long gcode_N, gcode_LastN;
 bool relative_mode = false;  //Determines Absolute or Relative Coordinates
-bool relative_mode_e = false;  //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
-long timediff = 0;
+
+//unsigned long steps_taken[NUM_AXIS];
+//long axis_interval[NUM_AXIS]; // for speed delay
+//float time_for_move;
+//bool relative_mode_e = false;  //Determines Absolute or Relative E Codes while in Absolute Coordinates mode. E is always relative in Relative Coordinates mode.
+//long timediff = 0;
 
 //experimental feedrate calc
 //float d = 0;
 //float axis_diff[NUM_AXIS] = {0, 0, 0, 0};
 
+
+#ifdef USE_ARC_FUNCTION
 //For arc centerpont, send bei Command G2/G3
 float offset[3] = {0.0, 0.0, 0.0};
+#endif
 
 #ifdef STEP_DELAY_RATIO
   long long_step_delay_ratio = STEP_DELAY_RATIO * 100;
@@ -195,9 +220,9 @@ float offset[3] = {0.0, 0.0, 0.0};
 #endif
 
 // comm variables and Commandbuffer
-// BUFSIZE is reduced from 8 to 5 to free more RAM for the PLANNER
+// BUFSIZE is reduced from 8 to 6 to free more RAM for the PLANNER
 #define MAX_CMD_SIZE 96
-#define BUFSIZE 5 //8
+#define BUFSIZE 6 //8
 char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];
 bool fromsd[BUFSIZE];
 
@@ -208,9 +233,9 @@ bool fromsd[BUFSIZE];
   long xferbytes;
 #endif
 
-int bufindr = 0;
-int bufindw = 0;
-int buflen = 0;
+unsigned char bufindr = 0;
+unsigned char bufindw = 0;
+unsigned char buflen = 0;
 char serial_char;
 int serial_count = 0;
 boolean comment_mode = false;
@@ -276,6 +301,11 @@ unsigned char manage_monitor = 255;
   }
   
   #ifdef SD_FAST_XFER_AKTIV
+  
+  #ifdef PIDTEMP
+    extern int g_heater_pwm_val;
+  #endif
+  
   void fast_xfer()
   {
     char *pstr;
@@ -285,6 +315,8 @@ unsigned char manage_monitor = 255;
     if(HEATER_0_PIN > -1) WRITE(HEATER_0_PIN,LOW);
     if(HEATER_1_PIN > -1) WRITE(HEATER_1_PIN,LOW);
     
+    g_heater_pwm_val = 0;
+    
     lastxferchar = 1;
     xferbytes = 0;
     
@@ -292,7 +324,7 @@ unsigned char manage_monitor = 255;
     
     if(pstr == NULL)
     {
-      Serial.println("invalid command");
+      showString(PSTR("invalid command\r\n"));
       return;
     }
     
@@ -301,24 +333,24 @@ unsigned char manage_monitor = 255;
     //check mode (currently only RAW is supported
     if(strcmp(strchr_pointer+4, "RAW") != 0)
     {
-      Serial.println("Invalid transfer codec");
+      showString(PSTR("Invalid transfer codec\r\n"));
       return;
     }else{
-      Serial.print("Selected codec: ");
+      showString(PSTR("Selected codec: "));
       Serial.println(strchr_pointer+4);
     }
     
     if (!file.open(&root, pstr+1, O_CREAT | O_APPEND | O_WRITE | O_TRUNC))
     {
-      Serial.print("open failed, File: ");
+      showString(PSTR("open failed, File: "));
       Serial.print(pstr+1);
-      Serial.print(".");
+      showString(PSTR("."));
     }else{
-      Serial.print("Writing to file: ");
+      showString(PSTR("Writing to file: "));
       Serial.println(pstr+1);
     }
         
-    Serial.println("ok");
+    showString(PSTR("ok\r\n"));
     
     //RAW transfer codec
     //Host sends \0 then up to SD_FAST_XFER_CHUNK_SIZE then \0
@@ -331,15 +363,14 @@ unsigned char manage_monitor = 255;
       while(!Serial.available())
       {
       }
-      if(Serial.peek() != 0)
+      if(Serial.read() != 0)
       {
         //host has failed, this isn't a RAW chunk, it's an actual command
         file.sync();
         file.close();
         return;
       }
-      //clear the initial 0
-      Serial.read();
+
       for(int i=0;i<SD_FAST_XFER_CHUNK_SIZE+1;i++)
       {
         while(!Serial.available())
@@ -359,11 +390,11 @@ unsigned char manage_monitor = 255;
       {
         fastxferbuffer[SD_FAST_XFER_CHUNK_SIZE] = 0;
         file.write(fastxferbuffer);
-        Serial.println("ok");
+        showString(PSTR("ok\r\n"));
       }else{
-        Serial.print("Wrote ");
+        showString(PSTR("Wrote "));
         Serial.print(xferbytes);
-        Serial.println(" bytes.");
+        showString(PSTR(" bytes.\r\n"));
         done = true;
       }
     }
@@ -417,7 +448,7 @@ unsigned char manage_monitor = 255;
     
  
   
-  inline void write_command(char *buf)
+  FORCE_INLINE void write_command(char *buf)
   {
       char* begin = buf;
       char* npos = 0;
@@ -622,13 +653,16 @@ void setup()
     SET_OUTPUT(E_STEP_PIN);
   #endif  
 
-  #ifdef RAMP_ACCELERATION
-  for(int i=0; i < NUM_AXIS; i++){
-        axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);
-        axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
-//        axis_travel_steps_per_sqr_second[i] = max_travel_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
-    }
-  #endif
+  for(int8_t i=0; i < NUM_AXIS; i++)
+  {
+    axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
+  }
+
+//  for(int i=0; i < NUM_AXIS; i++){
+//      axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);
+//      axis_steps_per_sqr_second[i] = max_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
+//      axis_travel_steps_per_sqr_second[i] = max_travel_acceleration_units_per_sq_second[i] * axis_steps_per_unit[i];
+//  }
     
 #ifdef HEATER_USES_MAX6675
   SET_OUTPUT(SCK_PIN);
@@ -716,7 +750,10 @@ void loop()
 #endif
 
     buflen = (buflen-1);
-    bufindr = (bufindr + 1)%BUFSIZE;
+    //bufindr = (bufindr + 1)%BUFSIZE;
+    //Removed modulo (%) operator, which uses an expensive divide and multiplication
+    bufindr++;
+    if(bufindr == BUFSIZE) bufindr = 0;
   }
   
   //check heater every n milliseconds
@@ -812,8 +849,10 @@ void get_command()
           {
             case 0:
             case 1:
+            #ifdef USE_ARC_FUNCTION
             case 2:  //G2
             case 3:  //G3 arc func
+            #endif
               #ifdef SDSUPPORT
               if(savetosd)
                 break;
@@ -826,7 +865,10 @@ void get_command()
             break;
           }
         }
-        bufindw = (bufindw + 1)%BUFSIZE;
+		//Removed modulo (%) operator, which uses an expensive divide and multiplication
+        //bufindw = (bufindw + 1)%BUFSIZE;
+        bufindw++;
+        if(bufindw == BUFSIZE) bufindw = 0;
         buflen += 1;
       }
       comment_mode = false; //for new command
@@ -861,7 +903,11 @@ void get_command()
         {
           fromsd[bufindw] = true;
           buflen += 1;
-          bufindw = (bufindw + 1)%BUFSIZE;
+		  //Removed modulo (%) operator, which uses an expensive divide and multiplication	
+          //bufindw = (bufindw + 1)%BUFSIZE;
+          bufindw++;
+          if(bufindw == BUFSIZE) bufindw = 0;
+          
         }
         comment_mode = false; //for new command
         serial_count = 0; //clear buffer
@@ -877,11 +923,11 @@ void get_command()
 }
 
 
-inline float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); }
-inline long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); }
-inline bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); }  //Return True if the string was found
+FORCE_INLINE float code_value() { return (strtod(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL)); }
+FORCE_INLINE long code_value_long() { return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10)); }
+FORCE_INLINE bool code_seen(char code_string[]) { return (strstr(cmdbuffer[bufindr], code_string) != NULL); }  //Return True if the string was found
 
-inline bool code_seen(char code)
+FORCE_INLINE bool code_seen(char code)
 {
   strchr_pointer = strchr(cmdbuffer[bufindr], code);
   return (strchr_pointer != NULL);  //Return True if a character was found
@@ -890,7 +936,7 @@ inline bool code_seen(char code)
 //------------------------------------------------
 // CHECK COMMAND AND CONVERT VALUES
 //------------------------------------------------
-inline void process_commands()
+FORCE_INLINE void process_commands()
 {
   unsigned long codenum; //throw away variable
   char *starpos = NULL;
@@ -910,6 +956,7 @@ inline void process_commands()
         //ClearToSend();
         return;
         //break;
+      #ifdef USE_ARC_FUNCTION
       case 2: // G2  - CW ARC
         get_arc_coordinates();
         prepare_arc_move(true);
@@ -922,6 +969,7 @@ inline void process_commands()
         previous_millis_cmd = millis();
         //break;
         return;  
+      #endif  
       case 4: // G4 dwell
         codenum = 0;
         if(code_seen('P')) codenum = code_value(); // milliseconds to wait
@@ -973,7 +1021,7 @@ inline void process_commands()
             feedrate = 0;
           }
         }
-        showString(PSTR("HOME X AXIS\r\n"));
+        //showString(PSTR("HOME X AXIS\r\n"));
 
         if((home_all_axis) || (code_seen(axis_codes[Y_AXIS]))) 
         {
@@ -1003,7 +1051,7 @@ inline void process_commands()
             feedrate = 0;
           }
         }
-        showString(PSTR("HOME Y AXIS\r\n"));
+        //showString(PSTR("HOME Y AXIS\r\n"));
 
         if((home_all_axis) || (code_seen(axis_codes[Z_AXIS]))) 
         {
@@ -1034,7 +1082,7 @@ inline void process_commands()
           }
         }    
    
-        showString(PSTR("HOME Z AXIS\r\n"));   
+        //showString(PSTR("HOME Z AXIS\r\n"));   
       
         feedrate = saved_feedrate;
         feedmultiply = saved_feedmultiply;
@@ -1266,12 +1314,14 @@ inline void process_commands()
           #ifdef PIDTEMP
             showString(PSTR(" @:"));
             Serial.print(heater_duty); 
+            /*
             showString(PSTR(",P:"));
             Serial.print(pTerm);
             showString(PSTR(",I:"));
             Serial.print(iTerm);
             showString(PSTR(",D:"));
             Serial.print(dTerm);
+            */
             #ifdef AUTOTEMP
               showString(PSTR(",AU:"));
               Serial.print(autotemp_setpoint);
@@ -1410,16 +1460,14 @@ inline void process_commands()
           if(code_seen(axis_codes[i])) axis_steps_per_unit[i] = code_value();
         }
         
-        //Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it
-        // should also be used in setup() as well
-        #ifdef RAMP_ACCELERATION
-          long temp_max_intervals[NUM_AXIS];
-          for(int i=0; i < NUM_AXIS; i++) 
-          {
-            axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);//TODO: do this for
-            // all steps_per_unit related variables
-          }
-        #endif
+          // Update start speed intervals and axis order. TODO: refactor axis_max_interval[] calculation into a function, as it
+          // should also be used in setup() as well
+//        long temp_max_intervals[NUM_AXIS];
+//        for(int i=0; i < NUM_AXIS; i++) 
+//        {
+//          axis_max_interval[i] = 100000000.0 / (max_start_speed_units_per_second[i] * axis_steps_per_unit[i]);//TODO: do this for
+//          all steps_per_unit related variables
+//        }
         break;
       case 115: // M115
         showString(PSTR("FIRMWARE_NAME: SprinterV2 PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:1\r\n"));
@@ -1466,25 +1514,47 @@ inline void process_commands()
       
         showString(PSTR("\r\n"));
       	break;
-      #ifdef RAMP_ACCELERATION
-      //TODO: update for all axis, use for loop
       case 201: // M201
-        for(int i=0; i < NUM_AXIS; i++) 
+
+        for(int8_t i=0; i < NUM_AXIS; i++) 
         {
-          if(code_seen(axis_codes[i])) axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
+          if(code_seen(axis_codes[i]))
+          {
+            max_acceleration_units_per_sq_second[i] = code_value();
+            axis_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
+          }
         }
-        break;
+
+      #if 0 // Not used for Sprinter/grbl gen6
       case 202: // M202
         for(int i=0; i < NUM_AXIS; i++) 
         {
           if(code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value() * axis_steps_per_unit[i];
         }
         break;
+      #else  
+      case 202: // M202 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;
       #endif
       case 203: // M203 Temperature monitor
-        if(code_seen('S')) manage_monitor = code_value();
-        if(manage_monitor==100) manage_monitor=1; // Set 100 to heated bed
-        break;
+          if(code_seen('S')) manage_monitor = code_value();
+          if(manage_monitor==100) manage_monitor=1; // Set 100 to heated bed
+      break;
+      case 204: // M204 acclereration S normal moves T filmanent only moves
+          if(code_seen('S')) move_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() ;
+      break;  
       case 220: // M220 S<factor in percent>- set speed factor override percentage
       {
         if(code_seen('S')) 
@@ -1563,7 +1633,7 @@ void ClearToSend()
   //Serial.println("ok");
 }
 
-inline void get_coordinates()
+FORCE_INLINE void get_coordinates()
 {
   for(int i=0; i < NUM_AXIS; i++)
   {
@@ -1578,12 +1648,14 @@ inline void get_coordinates()
   }
 }
 
-inline void get_arc_coordinates()
+#ifdef USE_ARC_FUNCTION
+FORCE_INLINE void get_arc_coordinates()
 {
    get_coordinates();
    if(code_seen('I')) offset[0] = code_value();
    if(code_seen('J')) offset[1] = code_value();
 }
+#endif
 
 
 void prepare_move()
@@ -1614,7 +1686,7 @@ void prepare_move()
 }
 
 
-
+#ifdef USE_ARC_FUNCTION
 void prepare_arc_move(char isclockwise) 
 {
 
@@ -1634,9 +1706,9 @@ void prepare_arc_move(char isclockwise)
     current_position[i] = destination[i];
   }
 }
+#endif
 
-
-inline void kill()
+FORCE_INLINE void kill()
 {
   #if TEMP_0_PIN > -1
     target_raw=0;
@@ -1657,7 +1729,7 @@ inline void kill()
   
 }
 
-inline void manage_inactivity(byte debug) 
+FORCE_INLINE void manage_inactivity(byte debug) 
 { 
   if( (millis()-previous_millis_cmd) >  max_inactive_time ) if(max_inactive_time) kill(); 
   
@@ -1712,19 +1784,43 @@ static block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for
 static volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 static volatile unsigned char block_buffer_tail;           // Index of the block to process now
 
+//===========================================================================
+//=============================private variables ============================
+//===========================================================================
+
+// Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+static int8_t next_block_index(int8_t block_index) {
+  block_index++;
+  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
+  return(block_index);
+}
+
+
+// Returns the index of the previous block in the ring buffer
+static int8_t prev_block_index(int8_t block_index) {
+  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
+  block_index--;
+  return(block_index);
+}
+
 // The current position of the tool in absolute steps
 static long position[4];   
+static float previous_speed[4]; // Speed of previous path line segment
+static float previous_nominal_speed; // Nominal speed of previous path line segment
 
-#define ONE_MINUTE_OF_MICROSECONDS 60000000.0
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
 // given acceleration:
-inline long estimate_acceleration_distance(long initial_rate, long target_rate, long acceleration)
+FORCE_INLINE float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
 {
-  return(
-  (target_rate*target_rate-initial_rate*initial_rate)/
-    (2L*acceleration)
-    );
+  if (acceleration!=0) {
+  return((target_rate*target_rate-initial_rate*initial_rate)/
+         (2.0*acceleration));
+  }
+  else {
+    return 0.0;  // acceleration was 0, set acceleration distance to 0
+  }
 }
 
 // This function gives you the point at which you must start braking (at the rate of -acceleration) if 
@@ -1732,166 +1828,143 @@ inline long estimate_acceleration_distance(long initial_rate, long target_rate,
 // a total travel of distance. This can be used to compute the intersection point between acceleration and
 // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
 
-inline long intersection_distance(long initial_rate, long final_rate, long acceleration, long distance)
+FORCE_INLINE float intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) 
 {
-  return(
-  (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
-    (4*acceleration)
-    );
+ if (acceleration!=0) {
+  return((2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/
+         (4.0*acceleration) );
+  }
+  else {
+    return 0.0;  // acceleration was 0, set intersection distance to 0
+  }
 }
 
 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
 
-void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed)
-{
-  if(block->busy == true) return; // If block is busy then bail out.
-  float entry_factor = entry_speed / block->nominal_speed;
-  float exit_factor = exit_speed / block->nominal_speed;
-  long initial_rate = ceil(block->nominal_rate*entry_factor);
-  long final_rate = ceil(block->nominal_rate*exit_factor);
-#ifdef ADVANCE
-  long initial_advance = block->advance*entry_factor*entry_factor;
-  long final_advance = block->advance*exit_factor*exit_factor;
-#endif // ADVANCE
+void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
+  unsigned long initial_rate = ceil(block->nominal_rate*entry_factor); // (step/min)
+  unsigned long final_rate = ceil(block->nominal_rate*exit_factor); // (step/min)
 
   // Limit minimal step rate (Otherwise the timer will overflow.)
-  if(initial_rate <120) initial_rate=120;
-  if(final_rate < 120) final_rate=120;
+  if(initial_rate <120) {initial_rate=120; }
+  if(final_rate < 120) {final_rate=120;  }
   
-  // Calculate the acceleration steps
   long acceleration = block->acceleration_st;
-  long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration);
-  long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration);
-  // Calculate the size of Plateau of Nominal Rate. 
-  long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
-
+  int32_t accelerate_steps =
+    ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration));
+  int32_t decelerate_steps =
+    floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration));
+    
+  // Calculate the size of Plateau of Nominal Rate.
+  int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
+  
   // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
-  // have to use intersection_distance() to calculate when to abort acceleration and start braking 
+  // have to use intersection_distance() to calculate when to abort acceleration and start braking
   // in order to reach the final_rate exactly at the end of this block.
-  if (plateau_steps < 0) {  
-    accelerate_steps = intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count);
+  if (plateau_steps < 0) {
+    accelerate_steps = ceil(
+      intersection_distance(block->initial_rate, block->final_rate, acceleration, block->step_event_count));
+    accelerate_steps = max(accelerate_steps,0); // Check limits due to numerical round-off
+    accelerate_steps = min(accelerate_steps,block->step_event_count);
     plateau_steps = 0;
-  }  
-
-  long decelerate_after = accelerate_steps+plateau_steps;
-  long acceleration_rate = (long)((float)acceleration * 8.388608);
+  }
 
+  #ifdef ADVANCE
+    volatile long initial_advance = block->advance*entry_factor*entry_factor; 
+    volatile long final_advance = block->advance*exit_factor*exit_factor;
+  #endif // ADVANCE
+  
+ // block->accelerate_until = accelerate_steps;
+ // block->decelerate_after = accelerate_steps+plateau_steps;
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
   if(block->busy == false) { // Don't update variables if block is busy.
     block->accelerate_until = accelerate_steps;
-    block->decelerate_after = decelerate_after;
-    block->acceleration_rate = acceleration_rate;
+    block->decelerate_after = accelerate_steps+plateau_steps;
     block->initial_rate = initial_rate;
     block->final_rate = final_rate;
-#ifdef ADVANCE
-    block->initial_advance = initial_advance;
-    block->final_advance = final_advance;
-#endif ADVANCE
+  #ifdef ADVANCE
+      block->initial_advance = initial_advance;
+      block->final_advance = final_advance;
+  #endif //ADVANCE
   }
   CRITICAL_SECTION_END;
 }                    
 
 // Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the 
 // acceleration within the allotted distance.
-inline float max_allowable_speed(float acceleration, float target_velocity, float distance) {
-  return(
-  sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance)
-    );
+FORCE_INLINE float max_allowable_speed(float acceleration, float target_velocity, float distance) {
+  return  sqrt(target_velocity*target_velocity-2*acceleration*distance);
 }
 
 // "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
 // This method will calculate the junction jerk as the euclidean distance between the nominal 
 // velocities of the respective blocks.
-inline float junction_jerk(block_t *before, block_t *after) {
-  return(sqrt(
-    pow((before->speed_x-after->speed_x), 2)+
-    pow((before->speed_y-after->speed_y), 2)));
-}
+//inline float junction_jerk(block_t *before, block_t *after) {
+//  return sqrt(
+//    pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
+//}
+
 
-// Return the safe speed which is max_jerk/2, e.g. the 
-// speed under which you cannot exceed max_jerk no matter what you do.
-float safe_speed(block_t *block) {
-  float safe_speed;
-  safe_speed = max_xy_jerk/2;  
-  if(abs(block->speed_z) > max_z_jerk/2) safe_speed = max_z_jerk/2;
-  if (safe_speed > block->nominal_speed) safe_speed = block->nominal_speed;
-  return safe_speed;  
-}
 
 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
 void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
-  if(!current) { 
-    return; 
-  }
-
-  float entry_speed = current->nominal_speed;
-  float exit_factor;
-  float exit_speed;
-  if (next) {
-    exit_speed = next->entry_speed;
-  } 
-  else {
-    exit_speed = safe_speed(current);
-  }
-
-  // Calculate the entry_factor for the current block. 
-  if (previous) {
-    // Reduce speed so that junction_jerk is within the maximum allowed
-    float jerk = junction_jerk(previous, current);
-    if((previous->steps_x == 0) && (previous->steps_y == 0)) {
-      entry_speed = safe_speed(current);
-    }
-    else if (jerk > max_xy_jerk) {
-      entry_speed = (max_xy_jerk/jerk) * entry_speed;
-    } 
-    if(abs(previous->speed_z - current->speed_z) > max_z_jerk) {
-      entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed;
-    }
-    // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
-    if (entry_speed > exit_speed) {
-      float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters);
-      if (max_entry_speed < entry_speed) {
-        entry_speed = max_entry_speed;
+  if(!current) { return; }
+  
+    if (next) {
+    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
+    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+    if (current->entry_speed != current->max_entry_speed) {
+    
+      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+      // for max allowable speed if block is decelerating and nominal length is false.
+      if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
+        current->entry_speed = min( current->max_entry_speed,
+          max_allowable_speed(-current->acceleration,next->entry_speed,current->millimeters));
+      } else {
+        current->entry_speed = current->max_entry_speed;
       }
-    }    
-  } 
-  else {
-    entry_speed = safe_speed(current);
-  }
-  // Store result
-  current->entry_speed = entry_speed;
+      current->recalculate_flag = true;
+    
+    }
+  } // Skip last block. Already initialized and set for recalculation.
 }
 
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the reverse pass.
 void planner_reverse_pass() {
-  char block_index = block_buffer_head;
-  block_t *block[3] = { NULL, NULL, NULL };
-  while(block_index != block_buffer_tail) {    
-    block_index--;
-    if(block_index < 0) block_index = BLOCK_BUFFER_SIZE-1;
-    block[2]= block[1];
-    block[1]= block[0];
-    block[0] = &block_buffer[block_index];
-    planner_reverse_pass_kernel(block[0], block[1], block[2]);
-  }
-  planner_reverse_pass_kernel(NULL, block[0], block[1]);
+  uint8_t block_index = block_buffer_head;
+  if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
+    block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
+    block_t *block[3] = { NULL, NULL, NULL };
+    while(block_index != block_buffer_tail) { 
+      block_index = prev_block_index(block_index); 
+      block[2]= block[1];
+      block[1]= block[0];
+      block[0] = &block_buffer[block_index];
+      planner_reverse_pass_kernel(block[0], block[1], block[2]);
+    }
+  }
 }
 
+
 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
 void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
-  if(!current) { 
-    return; 
-  }
-  if(previous) {
-    // If the previous block is an acceleration block, but it is not long enough to 
-    // complete the full speed change within the block, we need to adjust out entry
-    // speed accordingly. Remember current->entry_factor equals the exit factor of 
-    // the previous block.
-    if(previous->entry_speed < current->entry_speed) {
-      float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);
-      if (max_entry_speed < current->entry_speed) {
-        current->entry_speed = max_entry_speed;
+  if(!previous) { return; }
+  
+  // If the previous block is an acceleration block, but it is not long enough to complete the
+  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
+  // speeds have already been reset, maximized, and reverse planned by reverse planner.
+  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
+  if (!previous->nominal_length_flag) {
+    if (previous->entry_speed < current->entry_speed) {
+      double entry_speed = min( current->entry_speed,
+        max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->millimeters) );
+
+      // Check for junction speed change
+      if (current->entry_speed != entry_speed) {
+        current->entry_speed = entry_speed;
+        current->recalculate_flag = true;
       }
     }
   }
@@ -1900,16 +1973,15 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
 // planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This 
 // implements the forward pass.
 void planner_forward_pass() {
-  char block_index = block_buffer_tail;
-  block_t *block[3] = {
-    NULL, NULL, NULL  };
+  uint8_t block_index = block_buffer_tail;
+  block_t *block[3] = { NULL, NULL, NULL };
 
   while(block_index != block_buffer_head) {
     block[0] = block[1];
     block[1] = block[2];
     block[2] = &block_buffer[block_index];
     planner_forward_pass_kernel(block[0],block[1],block[2]);
-    block_index = (block_index+1) & BLOCK_BUFFER_MASK;
+    block_index = next_block_index(block_index);
   }
   planner_forward_pass_kernel(block[1], block[2], NULL);
 }
@@ -1918,18 +1990,30 @@ void planner_forward_pass() {
 // entry_factor for each junction. Must be called by planner_recalculate() after 
 // updating the blocks.
 void planner_recalculate_trapezoids() {
-  char block_index = block_buffer_tail;
+  int8_t block_index = block_buffer_tail;
   block_t *current;
   block_t *next = NULL;
+  
   while(block_index != block_buffer_head) {
     current = next;
     next = &block_buffer[block_index];
     if (current) {
-      calculate_trapezoid_for_block(current, current->entry_speed, next->entry_speed);      
+      // Recalculate if current block entry or exit junction speed has changed.
+      if (current->recalculate_flag || next->recalculate_flag) {
+        // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+        calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
+          next->entry_speed/current->nominal_speed);
+        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+      }
     }
-    block_index = (block_index+1) & BLOCK_BUFFER_MASK;
+    block_index = next_block_index( block_index );
+  }
+  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
+  if(next != NULL) {
+    calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed,
+      MINIMUM_PLANNER_SPEED/next->nominal_speed);
+    next->recalculate_flag = false;
   }
-  calculate_trapezoid_for_block(next, next->entry_speed, safe_speed(next));
 }
 
 // Recalculates the motion plan according to the following algorithm:
@@ -1959,16 +2043,22 @@ void plan_init() {
   block_buffer_head = 0;
   block_buffer_tail = 0;
   memset(position, 0, sizeof(position)); // clear position
+  previous_speed[0] = 0.0;
+  previous_speed[1] = 0.0;
+  previous_speed[2] = 0.0;
+  previous_speed[3] = 0.0;
+  previous_nominal_speed = 0.0;
 }
 
 
-inline void plan_discard_current_block() {
+
+FORCE_INLINE void plan_discard_current_block() {
   if (block_buffer_head != block_buffer_tail) {
     block_buffer_tail = (block_buffer_tail + 1) & BLOCK_BUFFER_MASK;  
   }
 }
 
-inline block_t *plan_get_current_block() {
+FORCE_INLINE block_t *plan_get_current_block() {
   if (block_buffer_head == block_buffer_tail) { 
     return(NULL); 
   }
@@ -1977,6 +2067,16 @@ inline block_t *plan_get_current_block() {
   return(block);
 }
 
+// Gets the current block. Returns NULL if buffer empty
+FORCE_INLINE bool blocks_queued() 
+{
+  if (block_buffer_head == block_buffer_tail) { 
+    return false; 
+  }
+  else
+    return true;
+}
+
 void check_axes_activity() {
   unsigned char x_active = 0;
   unsigned char y_active = 0;  
@@ -1985,14 +2085,14 @@ void check_axes_activity() {
   block_t *block;
 
   if(block_buffer_tail != block_buffer_head) {
-    char block_index = block_buffer_tail;
+    uint8_t block_index = block_buffer_tail;
     while(block_index != block_buffer_head) {
       block = &block_buffer[block_index];
       if(block->steps_x != 0) x_active++;
       if(block->steps_y != 0) y_active++;
       if(block->steps_z != 0) z_active++;
       if(block->steps_e != 0) e_active++;
-      block_index = (block_index+1) & BLOCK_BUFFER_MASK;
+      block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
     }
   }
   if((DISABLE_X) && (x_active == 0)) disable_x();
@@ -2001,21 +2101,15 @@ void check_axes_activity() {
   if((DISABLE_E) && (e_active == 0)) disable_e();
 }
 
+
+float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
-
-  // The target position of the tool in absolute steps
-  // Calculate target position in absolute steps
-  long target[4];
-  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
-  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
-  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
-  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);     
-  
+void plan_buffer_line(float x, float y, float z, float e, float feed_rate)
+{
   // Calculate the buffer head after we push this byte
-  int next_buffer_head = (block_buffer_head + 1) & BLOCK_BUFFER_MASK;	
+  int next_buffer_head = next_block_index(block_buffer_head);
 
   // If the buffer is full: good! That means we are well ahead of the robot. 
   // Rest here until there is room in the buffer.
@@ -2023,12 +2117,16 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
     manage_heater(); 
     manage_inactivity(1); 
   }
-  
-  //showString(PSTR("X:"));
-  //Serial.print(x);
-  //showString(PSTR(" Y:"));
-  //Serial.println(y);
 
+  // The target position of the tool in absolute steps
+  // Calculate target position in absolute steps
+  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
+  long target[4];
+  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
+  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
+  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
+  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
+  
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
   
@@ -2043,11 +2141,16 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
   block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
 
   // Bail if this is a zero-length block
-  if (block->step_event_count == 0) { 
-    return; 
-  };
-  
+  if (block->step_event_count <=dropsegments) { return; };
+
+  // Compute direction bits for this block 
+  block->direction_bits = 0;
+  if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); }
+  if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); }
+  if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); }
+  if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); }
   
+
  #ifdef DELAY_ENABLE
   if(block->steps_x != 0)
   {
@@ -2077,114 +2180,217 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
   if(block->steps_e != 0) enable_e();
  #endif 
 
-  float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
-  float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
-  float delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
-  float delta_e_mm = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
-  block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm) + square(delta_e_mm));
+  float delta_mm[4];
+  delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
+  delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
+  delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
+  delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
+  block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
+                            square(delta_mm[Z_AXIS]) + square(delta_mm[E_AXIS]));
+  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 
+  
+  // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
+  float inverse_second = feed_rate * inverse_millimeters;
+  
+  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
+  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
+
+  
+ 
+
+  if (block->steps_e == 0) {
+        if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
+  }
+  else {
+    	if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
+  } 
 
-  unsigned long microseconds;
-  microseconds = lround((block->millimeters/feed_rate)*1000000);
+#ifdef SLOWDOWN
+  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
+  int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
   
-  // Calculate speed in mm/minute for each axis
-  float multiplier = 60.0*1000000.0/microseconds;
-  block->speed_z = delta_z_mm * multiplier; 
-  block->speed_x = delta_x_mm * multiplier;
-  block->speed_y = delta_y_mm * multiplier;
-  block->speed_e = delta_e_mm * multiplier; 
+  if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); 
+#endif
+
+/*
+  //  segment time im micro seconds
+  long segment_time = lround(1000000.0/inverse_second);
+  if ((blockcount>0) && (blockcount < (BLOCK_BUFFER_SIZE - 4))) {
+    if (segment_time<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
+        segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/blockcount);
+    }
+  }
+  else {
+    if (segment_time<minsegmenttime) segment_time=minsegmenttime;
+  }
+  //  END OF SLOW DOWN SECTION    
+*/
+
+
+ // Calculate speed in mm/sec for each axis
+  float current_speed[4];
+  for(int i=0; i < 4; i++) {
+    current_speed[i] = delta_mm[i] * inverse_second;
+  }
 
   // Limit speed per axis
-  float speed_factor = 1;
-  float tmp_speed_factor;
-  if(abs(block->speed_x) > max_feedrate[X_AXIS]) {
-    speed_factor = max_feedrate[X_AXIS] / abs(block->speed_x);
-  }
-  if(abs(block->speed_y) > max_feedrate[Y_AXIS]){
-    tmp_speed_factor = max_feedrate[Y_AXIS] / abs(block->speed_y);
-    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
-  }
-  if(abs(block->speed_z) > max_feedrate[Z_AXIS]){
-    tmp_speed_factor = max_feedrate[Z_AXIS] / abs(block->speed_z);
-    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
-  }
-  if(abs(block->speed_e) > max_feedrate[E_AXIS]){
-    tmp_speed_factor = max_feedrate[E_AXIS] / abs(block->speed_e);
-    if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
-  }
-  multiplier = multiplier * speed_factor;
-  block->speed_z = delta_z_mm * multiplier; 
-  block->speed_x = delta_x_mm * multiplier;
-  block->speed_y = delta_y_mm * multiplier;
-  block->speed_e = delta_e_mm * multiplier; 
-  block->nominal_speed = block->millimeters * multiplier;
-  block->nominal_rate = ceil(block->step_event_count * multiplier / 60);  
-  
-  if(block->nominal_rate < 120) block->nominal_rate = 120;
-  block->entry_speed = safe_speed(block);
-
-  // Compute the acceleration rate for the trapezoid generator. 
-  float travel_per_step = block->millimeters/block->step_event_count;
+  float speed_factor = 1.0; //factor <=1 do decrease speed
+  for(int i=0; i < 4; i++) {
+    if(abs(current_speed[i]) > max_feedrate[i])
+      speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
+  }
+
+  // Correct the speed  
+  if( speed_factor < 1.0) {
+//    Serial.print("speed factor : "); Serial.println(speed_factor);
+    for(int i=0; i < 4; i++) {
+    if(abs(current_speed[i]) > max_feedrate[i])
+      speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
+ /*     
+      if(speed_factor < 0.1) {
+        Serial.print("speed factor : "); Serial.println(speed_factor);
+        Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]);
+      }
+ */
+  }
+    for(unsigned char i=0; i < 4; i++) {
+      current_speed[i] *= speed_factor;
+    }
+    block->nominal_speed *= speed_factor;
+    block->nominal_rate *= speed_factor;
+  }
+
+  // Compute and limit the acceleration rate for the trapezoid generator.  
+  float steps_per_mm = block->step_event_count/block->millimeters;
   if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
-    block->acceleration_st = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
+    block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
   }
   else {
-    block->acceleration_st = ceil( (acceleration)/travel_per_step);      // convert to: acceleration steps/sec^2
+    block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
     // Limit acceleration per axis
-    if((block->acceleration_st * block->steps_x / block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
+    if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
       block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
-    if((block->acceleration_st * block->steps_y / block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
+    if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
       block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
-    if((block->acceleration_st * block->steps_e / block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
+    if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
       block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
-    if(((block->acceleration_st / block->step_event_count) * block->steps_z ) > axis_steps_per_sqr_second[Z_AXIS])
+    if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
       block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
   }
-  block->acceleration = block->acceleration_st * travel_per_step;
-  
-#ifdef ADVANCE
-  // Calculate advance rate
-  if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
-    block->advance_rate = 0;
-    block->advance = 0;
+  block->acceleration = block->acceleration_st / steps_per_mm;
+  block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
+  
+#if 0  // Use old jerk for now
+  // Compute path unit vector
+  double unit_vec[3];
+
+  unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
+  unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
+  unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
+  
+  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+  // Let a circle be tangent to both previous and current path line segments, where the junction
+  // deviation is defined as the distance from the junction to the closest edge of the circle,
+  // colinear with the circle center. The circular segment joining the two paths represents the
+  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
+  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
+  // path width or max_jerk in the previous grbl version. This approach does not actually deviate
+  // from path, but used as a robust way to compute cornering speeds, as it takes into account the
+  // nonlinearities of both the junction angle and junction velocity.
+  double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+
+  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
+  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+    // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
+    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
+    double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
+                       - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
+                       - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+                           
+    // Skip and use default max junction speed for 0 degree acute junction.
+    if (cos_theta < 0.95) {
+      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
+      // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+      if (cos_theta > -0.95) {
+        // Compute maximum junction velocity based on maximum acceleration and junction deviation
+        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+        vmax_junction = min(vmax_junction,
+          sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
+      }
+    }
   }
-  else {
-    long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
-    float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
-      (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;
-    block->advance = advance;
-    if(acc_dist == 0) {
-      block->advance_rate = 0;
+#endif
+  // Start with a safe speed
+  float vmax_junction = max_xy_jerk/2;  
+  if(abs(current_speed[Z_AXIS]) > max_z_jerk/2) 
+    vmax_junction = max_z_jerk/2;
+  vmax_junction = min(vmax_junction, block->nominal_speed);
+
+  if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+    float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
+    if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) {
+      vmax_junction = block->nominal_speed;
+    }
+    if (jerk > max_xy_jerk) {
+      vmax_junction *= (max_xy_jerk/jerk);
     } 
+    if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
+      vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
+    } 
+  }
+  block->max_entry_speed = vmax_junction;
+    
+  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+  double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
+  block->entry_speed = min(vmax_junction, v_allowable);
+
+  // Initialize planner efficiency flags
+  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
+  // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+  // the current block and next block junction speeds are guaranteed to always be at their maximum
+  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+  // the reverse and forward planners, the corresponding block junction speed will always be at the
+  // the maximum junction speed and may always be ignored for any speed reduction checks.
+  if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
+  else { block->nominal_length_flag = false; }
+  block->recalculate_flag = true; // Always calculate trapezoid for new block
+  
+  // Update previous path unit_vector and nominal speed
+  memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
+  previous_nominal_speed = block->nominal_speed;
+  
+  #ifdef ADVANCE
+    // Calculate advance rate
+    if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
+      block->advance_rate = 0;
+      block->advance = 0;
+    }
     else {
-      block->advance_rate = advance / (float)acc_dist;
+      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
+      float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
+        (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
+      block->advance = advance;
+      if(acc_dist == 0) {
+        block->advance_rate = 0;
+      } 
+      else {
+        block->advance_rate = advance / (float)acc_dist;
+      }
     }
-  }
 
-#endif // ADVANCE
+  #endif // ADVANCE
 
-  // compute a preliminary conservative acceleration trapezoid
-  float safespeed = safe_speed(block);
-  calculate_trapezoid_for_block(block, safespeed, safespeed); 
 
-  // Compute direction bits for this block
-  block->direction_bits = 0;
-  if (target[X_AXIS] < position[X_AXIS]) { 
-    block->direction_bits |= (1<<X_AXIS); 
-  }
-  if (target[Y_AXIS] < position[Y_AXIS]) { 
-    block->direction_bits |= (1<<Y_AXIS); 
-  }
-  if (target[Z_AXIS] < position[Z_AXIS]) { 
-    block->direction_bits |= (1<<Z_AXIS); 
-  }
-  if (target[E_AXIS] < position[E_AXIS]) { 
-    block->direction_bits |= (1<<E_AXIS); 
-  }
 
-  // Move buffer head
-  block_buffer_head = next_buffer_head;     
 
-  // Update position 
+  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
+    MINIMUM_PLANNER_SPEED/block->nominal_speed);
+    
+  // Move buffer head
+  block_buffer_head = next_buffer_head;
+  
+  // Update position
   memcpy(position, target, sizeof(target)); // position[] = target[]
 
   planner_recalculate();
@@ -2199,7 +2405,13 @@ void plan_set_position(float x, float y, float z, float e)
   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
   position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
-  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);     
+  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
+  st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
+  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+  previous_speed[0] = 0.0;
+  previous_speed[1] = 0.0;
+  previous_speed[2] = 0.0;
+  previous_speed[3] = 0.0;
 }
 
 #ifdef AUTOTEMP
@@ -2323,6 +2535,12 @@ asm volatile ( \
 #define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
 
+#ifdef ENDSTOPS_ONLY_FOR_HOMING
+  #define CHECK_ENDSTOPS  if(check_endstops)
+#else
+  #define CHECK_ENDSTOPS
+#endif
+
 static block_t *current_block;  // A pointer to the block currently being traced
 
 // Variables used by The Stepper Driver Interrupt
@@ -2337,9 +2555,27 @@ static short old_advance = 0;
 static short e_steps;
 static unsigned char busy = false; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
 static long acceleration_time, deceleration_time;
-static long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
+//static long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
 static unsigned short acc_step_rate; // needed for deccelaration start point
+static char step_loops;
+static unsigned short OCR1A_nominal;
+
+volatile long endstops_trigsteps[3]={0,0,0};
+static volatile bool endstop_x_hit=false;
+static volatile bool endstop_y_hit=false;
+static volatile bool endstop_z_hit=false;
+
+static bool old_x_min_endstop=false;
+static bool old_x_max_endstop=false;
+static bool old_y_min_endstop=false;
+static bool old_y_max_endstop=false;
+static bool old_z_min_endstop=false;
+static bool old_z_max_endstop=false;
 
+static bool check_endstops = true;
+
+volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
+volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
 
 
 //         __________________________
@@ -2361,16 +2597,36 @@ static unsigned short acc_step_rate; // needed for deccelaration start point
 void st_wake_up() 
 {
   //  TCNT1 = 0;
+  if(busy == false) 
   ENABLE_STEPPER_DRIVER_INTERRUPT();  
 }
 
-inline unsigned short calc_timer(unsigned short step_rate) 
+void enable_endstops(bool check)
+{
+  check_endstops = check;
+}
+
+FORCE_INLINE unsigned short calc_timer(unsigned short step_rate)
 {
   unsigned short timer;
+  if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
+  
+  if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
+    step_rate = (step_rate >> 2)&0x3fff;
+    step_loops = 4;
+  }
+  else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
+    step_rate = (step_rate >> 1)&0x7fff;
+    step_loops = 2;
+  }
+  else {
+    step_loops = 1;
+  } 
+  
   if(step_rate < 32) step_rate = 32;
   step_rate -= 32; // Correct for minimal speed
   
-  if(step_rate >= (8*256))
+  if(step_rate >= (8*256)) // higher step rate 
   { // higher step rate 
     unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
     unsigned char tmp_step_rate = (step_rate & 0x00ff);
@@ -2385,48 +2641,36 @@ inline unsigned short calc_timer(unsigned short step_rate)
     timer = (unsigned short)pgm_read_word_near(table_address);
     timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
   }
-  
-  if(timer < 100) timer = 100;
-  
+  if(timer < 100) { timer = 100; }//(20kHz this should never happen)
   return timer;
 }
 
 // Initializes the trapezoid generator from the current block. Called whenever a new 
 // block begins.
-inline void trapezoid_generator_reset()
+FORCE_INLINE void trapezoid_generator_reset()
 {
-  accelerate_until = current_block->accelerate_until;
-  decelerate_after = current_block->decelerate_after;
-  acceleration_rate = current_block->acceleration_rate;
-  initial_rate = current_block->initial_rate;
-  final_rate = current_block->final_rate;
-  nominal_rate = current_block->nominal_rate;
-
   #ifdef ADVANCE
     advance = current_block->initial_advance;
     final_advance = current_block->final_advance;
-    advance_rate = current_block->advance_rate;
+    // Do E steps + advance steps
+    e_steps += ((advance >>8) - old_advance);
+    old_advance = advance >>8;  
   #endif
   deceleration_time = 0;
   
   
   // step_rate to timer interval
-  acc_step_rate = initial_rate;
+  acc_step_rate = current_block->initial_rate;
   acceleration_time = calc_timer(acc_step_rate);
   OCR1A = acceleration_time;
+  OCR1A_nominal = calc_timer(current_block->nominal_rate);
+    
 }
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.  
 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
 ISR(TIMER1_COMPA_vect)
 {        
-  if(busy){ /*Serial.println("BUSY")*/; 
-    return; 
-  } // The busy-flag is used to avoid reentering this interrupt
-
-  busy = true;
-  sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
-
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {
     // Anything in the buffer?
@@ -2438,10 +2682,12 @@ ISR(TIMER1_COMPA_vect)
       counter_z = counter_x;
       counter_e = counter_x;
       step_events_completed = 0;
-      e_steps = 0;
+//      #ifdef ADVANCE
+//      e_steps = 0;
+//      #endif
     } 
     else {
-      DISABLE_STEPPER_DRIVER_INTERRUPT();
+        OCR1A=2000; // 1kHz.
     }    
   } 
 
@@ -2449,143 +2695,236 @@ ISR(TIMER1_COMPA_vect)
     // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
     out_bits = current_block->direction_bits;
 
-#ifdef ADVANCE
-    // Calculate E early.
-    counter_e += current_block->steps_e;
-    if (counter_e > 0) {
-      counter_e -= current_block->step_event_count;
-      if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
-        CRITICAL_SECTION_START;
-        e_steps--;
-        CRITICAL_SECTION_END;
-      }
-      else {
-        CRITICAL_SECTION_START;
-        e_steps++;
-        CRITICAL_SECTION_END;
-      }
-    }    
-    // Do E steps + advance steps
-    CRITICAL_SECTION_START;
-    e_steps += ((advance >> 16) - old_advance);
-    CRITICAL_SECTION_END;
-    old_advance = advance >> 16;  
-#endif //ADVANCE
-
     // Set direction en check limit switches
     if ((out_bits & (1<<X_AXIS)) != 0) {   // -direction
       WRITE(X_DIR_PIN, INVERT_X_DIR);
-      if(READ(X_MIN_PIN) != X_ENDSTOP_INVERT) {
-        step_events_completed = current_block->step_event_count;
+      count_direction[X_AXIS]=-1;
+      CHECK_ENDSTOPS
+      {
+        #if X_MIN_PIN > -1
+          bool x_min_endstop=(READ(X_MIN_PIN) != X_ENDSTOP_INVERT);
+          if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
+            endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
+            endstop_x_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_x_min_endstop = x_min_endstop;
+        #endif
+      }
+    }
+    else { // +direction 
+      WRITE(X_DIR_PIN,!INVERT_X_DIR);
+      count_direction[X_AXIS]=1;
+      CHECK_ENDSTOPS 
+      {
+        #if X_MAX_PIN > -1
+          bool x_max_endstop=(READ(X_MAX_PIN) != X_ENDSTOP_INVERT);
+          if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
+            endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
+            endstop_x_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_x_max_endstop = x_max_endstop;
+        #endif
       }
     }
-    else // +direction
-    WRITE(X_DIR_PIN,!INVERT_X_DIR);
 
     if ((out_bits & (1<<Y_AXIS)) != 0) {   // -direction
       WRITE(Y_DIR_PIN,INVERT_Y_DIR);
-      if(READ(Y_MIN_PIN) != Y_ENDSTOP_INVERT) {
-        step_events_completed = current_block->step_event_count;
+      count_direction[Y_AXIS]=-1;
+      CHECK_ENDSTOPS
+      {
+        #if Y_MIN_PIN > -1
+          bool y_min_endstop=(READ(Y_MIN_PIN) != Y_ENDSTOP_INVERT);
+          if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
+            endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
+            endstop_y_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_y_min_endstop = y_min_endstop;
+        #endif
       }
     }
-    else // +direction
+    else { // +direction
     WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
+      count_direction[Y_AXIS]=1;
+      CHECK_ENDSTOPS
+      {
+        #if Y_MAX_PIN > -1
+          bool y_max_endstop=(READ(Y_MAX_PIN) != Y_ENDSTOP_INVERT);
+          if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
+            endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
+            endstop_y_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_y_max_endstop = y_max_endstop;
+        #endif
+      }
+    }
 
     if ((out_bits & (1<<Z_AXIS)) != 0) {   // -direction
       WRITE(Z_DIR_PIN,INVERT_Z_DIR);
-      if(READ(Z_MIN_PIN) != Z_ENDSTOP_INVERT) {
-        step_events_completed = current_block->step_event_count;
+      count_direction[Z_AXIS]=-1;
+      CHECK_ENDSTOPS
+      {
+        #if Z_MIN_PIN > -1
+          bool z_min_endstop=(READ(Z_MIN_PIN) != Z_ENDSTOP_INVERT);
+          if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
+            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+            endstop_z_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_z_min_endstop = z_min_endstop;
+        #endif
       }
     }
-    else // +direction
-    WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
-
-#ifndef ADVANCE
-    if ((out_bits & (1<<E_AXIS)) != 0)   // -direction
-      WRITE(E_DIR_PIN,INVERT_E_DIR);
-    else // +direction
-      WRITE(E_DIR_PIN,!INVERT_E_DIR);
-#endif //!ADVANCE
-
-    counter_x += current_block->steps_x;
-    if (counter_x > 0) {
-      WRITE(X_STEP_PIN, HIGH);
-      counter_x -= current_block->step_event_count;
-      WRITE(X_STEP_PIN, LOW);
+    else { // +direction
+      WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
+      count_direction[Z_AXIS]=1;
+      CHECK_ENDSTOPS
+      {
+        #if Z_MAX_PIN > -1
+          bool z_max_endstop=(READ(Z_MAX_PIN) != Z_ENDSTOP_INVERT);
+          if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
+            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
+            endstop_z_hit=true;
+            step_events_completed = current_block->step_event_count;
+          }
+          old_z_max_endstop = z_max_endstop;
+        #endif
+      }
     }
 
-    counter_y += current_block->steps_y;
-    if (counter_y > 0) {
-      WRITE(Y_STEP_PIN, HIGH);
-      counter_y -= current_block->step_event_count;
-      WRITE(Y_STEP_PIN, LOW);
-    }
+    #ifndef ADVANCE
+      if ((out_bits & (1<<E_AXIS)) != 0) {  // -direction
+        WRITE(E_DIR_PIN,INVERT_E_DIR);
+        count_direction[E_AXIS]=-1;
+      }
+      else { // +direction
+        WRITE(E_DIR_PIN,!INVERT_E_DIR);
+        count_direction[E_AXIS]=-1;
+      }
+    #endif //!ADVANCE
+    
 
-    counter_z += current_block->steps_z;
-    if (counter_z > 0) {
-      WRITE(Z_STEP_PIN, HIGH);
-      counter_z -= current_block->step_event_count;
-      WRITE(Z_STEP_PIN, LOW);
-    }
+    
+    for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) 
+      //MSerial.checkRx(); // Check for serial chars. 
+      
+      #ifdef ADVANCE
+      counter_e += current_block->steps_e;
+      if (counter_e > 0) {
+        counter_e -= current_block->step_event_count;
+        if ((out_bits & (1<<E_AXIS)) != 0) { // - direction
+          e_steps--;
+        }
+        else {
+          e_steps++;
+        }
+      }    
+      #endif //ADVANCE
+      
+      counter_x += current_block->steps_x;
+      if (counter_x > 0) {
+        WRITE(X_STEP_PIN, HIGH);
+        counter_x -= current_block->step_event_count;
+        WRITE(X_STEP_PIN, LOW);
+        count_position[X_AXIS]+=count_direction[X_AXIS];   
+      }
 
-#ifndef ADVANCE
-    counter_e += current_block->steps_e;
-    if (counter_e > 0) {
-      WRITE(E_STEP_PIN, HIGH);
-      counter_e -= current_block->step_event_count;
-      WRITE(E_STEP_PIN, LOW);
-    }
-#endif //!ADVANCE
+      counter_y += current_block->steps_y;
+      if (counter_y > 0) {
+        WRITE(Y_STEP_PIN, HIGH);
+        counter_y -= current_block->step_event_count;
+        WRITE(Y_STEP_PIN, LOW);
+        count_position[Y_AXIS]+=count_direction[Y_AXIS];
+      }
+
+      counter_z += current_block->steps_z;
+      if (counter_z > 0) {
+        WRITE(Z_STEP_PIN, HIGH);
+        counter_z -= current_block->step_event_count;
+        WRITE(Z_STEP_PIN, LOW);
+        count_position[Z_AXIS]+=count_direction[Z_AXIS];
+      }
 
+      #ifndef ADVANCE
+        counter_e += current_block->steps_e;
+        if (counter_e > 0) {
+          WRITE(E_STEP_PIN, HIGH);
+          counter_e -= current_block->step_event_count;
+           WRITE(E_STEP_PIN, LOW);
+          count_position[E_AXIS]+=count_direction[E_AXIS];
+        }
+      #endif //!ADVANCE
+      step_events_completed += 1;  
+      if(step_events_completed >= current_block->step_event_count) break;
+    }
     // Calculare new timer value
     unsigned short timer;
     unsigned short step_rate;
-    if (step_events_completed < accelerate_until) {
-      MultiU24X24toH16(acc_step_rate, acceleration_time, acceleration_rate);
-      acc_step_rate += initial_rate;
+    if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
+      
+      MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
+      acc_step_rate += current_block->initial_rate;
       
       // upper limit
-      if(acc_step_rate > nominal_rate)
-        acc_step_rate = nominal_rate;
+      if(acc_step_rate > current_block->nominal_rate)
+        acc_step_rate = current_block->nominal_rate;
 
       // step_rate to timer interval
       timer = calc_timer(acc_step_rate);
-      advance += advance_rate;
-      acceleration_time += timer;
       OCR1A = timer;
+      acceleration_time += timer;
+      #ifdef ADVANCE
+        for(int8_t i=0; i < step_loops; i++) {
+          advance += advance_rate;
+        }
+        //if(advance > current_block->advance) advance = current_block->advance;
+        // Do E steps + advance steps
+        e_steps += ((advance >>8) - old_advance);
+        old_advance = advance >>8;  
+        
+      #endif
     } 
-    else if (step_events_completed >= decelerate_after) {   
-      MultiU24X24toH16(step_rate, deceleration_time, acceleration_rate);
+    else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {   
+      MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
       
       if(step_rate > acc_step_rate) { // Check step_rate stays positive
-        step_rate = final_rate;
+        step_rate = current_block->final_rate;
       }
       else {
         step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
       }
 
       // lower limit
-      if(step_rate < final_rate)
-        step_rate = final_rate;
+      if(step_rate < current_block->final_rate)
+        step_rate = current_block->final_rate;
 
       // step_rate to timer interval
       timer = calc_timer(step_rate);
-#ifdef ADVANCE
-      advance -= advance_rate;
-      if(advance < final_advance)
-        advance = final_advance;
-#endif //ADVANCE
-      deceleration_time += timer;
       OCR1A = timer;
-    }       
+      deceleration_time += timer;
+      #ifdef ADVANCE
+        for(int8_t i=0; i < step_loops; i++) {
+          advance -= advance_rate;
+        }
+        if(advance < final_advance) advance = final_advance;
+        // Do E steps + advance steps
+        e_steps += ((advance >>8) - old_advance);
+        old_advance = advance >>8;  
+      #endif //ADVANCE
+    }
+    else {
+      OCR1A = OCR1A_nominal;
+    }
+
     // If current block is finished, reset pointer 
-    step_events_completed += 1;  
     if (step_events_completed >= current_block->step_event_count) {
       current_block = NULL;
       plan_discard_current_block();
     }   
   } 
-  busy=false;
 }
 
 #ifdef ADVANCE
@@ -2595,23 +2934,27 @@ unsigned char old_OCR0A;
 // Timer 0 is shared with millies
 ISR(TIMER0_COMPA_vect)
 {
-  // Critical section needed because Timer 1 interrupt has higher priority. 
-  // The pin set functions are placed on trategic position to comply with the stepper driver timing.
-  WRITE(E_STEP_PIN, LOW);
+    old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
+    OCR0A = old_OCR0A;
   // Set E direction (Depends on E direction + advance)
-  if (e_steps < 0) {
-    WRITE(E_DIR_PIN,INVERT_E_DIR);    
-    e_steps++;
-    WRITE(E_STEP_PIN, HIGH);
-  } 
-  if (e_steps > 0) {
-    WRITE(E_DIR_PIN,!INVERT_E_DIR);
-    e_steps--;
-    WRITE(E_STEP_PIN, HIGH);
+  for(unsigned char i=0; i<4;i++) 
+  {
+      if (e_steps != 0)
+      {
+        WRITE(E0_STEP_PIN, LOW);
+        if (e_steps < 0) {
+          WRITE(E0_DIR_PIN, INVERT_E0_DIR);
+          e_steps++;
+          WRITE(E0_STEP_PIN, HIGH);
+        } 
+        else if (e_steps > 0) {
+          WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
+       	  e_steps--;
+    	  WRITE(E0_STEP_PIN, HIGH);
+  	    }
+      }
+    }
   }
-  old_OCR0A += 25; // 10kHz interrupt
-  OCR0A = old_OCR0A;
-}
 #endif // ADVANCE
 
 void st_init()
@@ -2628,24 +2971,46 @@ void st_init()
   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10); // 2MHz timer
 
   OCR1A = 0x4000;
-  DISABLE_STEPPER_DRIVER_INTERRUPT();  
+  TCNT1 = 0;
+  ENABLE_STEPPER_DRIVER_INTERRUPT();
 
 #ifdef ADVANCE
+  #if defined(TCCR0A) && defined(WGM01)
+    TCCR0A &= ~(1<<WGM01);
+    TCCR0A &= ~(1<<WGM00);
+  #endif  
   e_steps = 0;
   TIMSK0 |= (1<<OCIE0A);
 #endif //ADVANCE
+
+  #ifdef ENDSTOPS_ONLY_FOR_HOMING
+    enable_endstops(false);
+  #else
+    enable_endstops(true);
+  #endif
+  
   sei();
 }
 
 // Block until all buffered steps are executed
 void st_synchronize()
 {
-  while(plan_get_current_block()) {
+  while(blocks_queued()) {
     manage_heater();
     manage_inactivity(1);
   }   
 }
 
+void st_set_position(const long &x, const long &y, const long &z, const long &e)
+{
+  CRITICAL_SECTION_START;
+  count_position[X_AXIS] = x;
+  count_position[Y_AXIS] = y;
+  count_position[Z_AXIS] = z;
+  count_position[E_AXIS] = e;
+  CRITICAL_SECTION_END;
+}
+
 
 #ifdef DEBUG
 void log_message(char*   message) {