/*
    This file is part of Repetier-Firmware.

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

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

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

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

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

#include "Reptier.h"
#include "ui.h"

// ##########################################################################
// ###                         Path planner stuff                         ###
// ##########################################################################

inline unsigned long U16SquaredToU32(unsigned int val) {
  long res;
   __asm__ __volatile__ ( // 15 Ticks
   "mul %A1,%A1 \n\t"
   "movw %A0,r0 \n\t"
   "mul %B1,%B1 \n\t"
   "movw %C0,r0 \n\t"
   "mul %A1,%B1 \n\t"
   "clr %A1 \n\t"
   "add %B0,r0 \n\t"
   "adc %C0,r1 \n\t"
   "adc %D0,%A1 \n\t"
   "add %B0,r0 \n\t"
   "adc %C0,r1 \n\t"
   "adc %D0,%A1 \n\t"
   "clr r1 \n\t"
  : "=&r"(res),"=r"(val)
  : "1"(val)
   );
  return res;
}
/**
Computes the maximum junction speed

p1 = previous segment
p2 = new segment
*/
inline void computeMaxJunctionSpeed(PrintLine *p1,PrintLine *p2) {
  if(p1->flags & FLAG_WARMUP) {
    p2->joinFlags |= FLAG_JOIN_START_FIXED;
    return;
  }
#if DRIVE_SYSTEM==3
  if (p1->moveID == p2->moveID) { // Avoid computing junction speed for split delta lines
   if(p1->fullSpeed>p2->fullSpeed) 
      p1->maxJunctionSpeed = p2->fullSpeed;
   else
      p1->maxJunctionSpeed = p1->fullSpeed;
   return;
  }
#endif
   // First we compute the normalized jerk for speed 1
   float dx = p2->speedX-p1->speedX;
   float dy = p2->speedY-p1->speedY;
   float factor=1,tmp;
#if (DRIVE_SYSTEM == 3) // No point computing Z Jerk separately for delta moves
   float dz = p2->speedZ-p1->speedZ;
   float jerk = sqrt(dx*dx+dy*dy+dz*dz);
#else
   float jerk = sqrt(dx*dx+dy*dy);
#endif
   //if(DEBUG_ECHO) {OUT_P_F_LN("Jerk:",jerk);OUT_P_F_LN("FS:",p1->fullSpeed);OUT_P_F_LN("MaxJerk:",printer_state.maxJerk);}
   if(jerk>printer_state.maxJerk)
     factor = printer_state.maxJerk/jerk;
#if (DRIVE_SYSTEM!=3)
   if((p1->dir & 64) || (p2->dir & 64)) {
   //  float dz = (p2->speedZ*p2->invFullSpeed-p1->speedZ*p1->invFullSpeed)*printer_state.maxJerk/printer_state.maxZJerk;
     float dz = fabs(p2->speedZ-p1->speedZ);
     if(dz>printer_state.maxZJerk) {
       tmp = printer_state.maxZJerk/dz;
       if(tmp<factor) factor = tmp;
     }
   }
#endif
   float eJerk = fabs(p2->speedE-p1->speedE);
   if(eJerk>current_extruder->maxStartFeedrate) {
     tmp = current_extruder->maxStartFeedrate/eJerk;
     if(tmp<factor) factor = tmp;
   }
   p1->maxJunctionSpeed = p1->fullSpeed*factor;   
   if(p1->maxJunctionSpeed>p2->fullSpeed) p1->maxJunctionSpeed = p2->fullSpeed;
   //if(DEBUG_ECHO) OUT_P_F_LN("Factor:",factor);
   //if(DEBUG_ECHO) OUT_P_F_LN("JSPD:",p1->maxJunctionSpeed);
}

/** Update parameter used by updateTrapezoids

Computes the acceleration/decelleration steps and advanced parameter associated.
*/
void updateStepsParameter(PrintLine *p/*,byte caller*/) {
    if(p->flags & FLAG_WARMUP) return;
    if(p->joinFlags & FLAG_JOIN_STEPPARAMS_COMPUTED) return; // Already up to date, spare time
    float startFactor = p->startSpeed * p->invFullSpeed;
    float endFactor   = p->endSpeed   * p->invFullSpeed;
    p->vStart = p->vMax*startFactor; //starting speed
    p->vEnd   = p->vMax*endFactor;
    unsigned long vmax2 = U16SquaredToU32(p->vMax);
    p->accelSteps = ((vmax2-U16SquaredToU32(p->vStart))/(p->accelerationPrim<<1))+1; // Always add 1 for missing precision
    p->decelSteps = ((vmax2-U16SquaredToU32(p->vEnd))  /(p->accelerationPrim<<1))+1;
#ifdef USE_ADVANCE
#ifdef ENABLE_QUADRATIC_ADVANCE
    p->advanceStart = (float)p->advanceFull*startFactor * startFactor;
    p->advanceEnd   = (float)p->advanceFull*endFactor   * endFactor;
#endif
#endif
    if(p->accelSteps+p->decelSteps>=p->stepsRemaining) { // can't reach limit speed
      unsigned int red = (p->accelSteps+p->decelSteps+2-p->stepsRemaining)>>1;
      if(red<p->accelSteps)
        p->accelSteps-=red;
      else
        p->accelSteps = 0;
      if(red<p->decelSteps) {
        p->decelSteps-=red;
      } else
        p->decelSteps = 0;
    }
    p->joinFlags|=FLAG_JOIN_STEPPARAMS_COMPUTED;
#ifdef DEBUG_QUEUE_MOVE
    if(DEBUG_ECHO) {
      out.print_int_P(PSTR("ID:"),(int)p);
      out.print_int_P(PSTR("vStart/End:"),p->vStart);
      out.println_int_P(PSTR("/"),p->vEnd);
      out.print_int_P(PSTR("accel/decel steps:"),p->accelSteps);
      out.println_int_P(PSTR("/"),p->decelSteps);
      out.print_float_P(PSTR("st./end speed:"),p->startSpeed);
      out.println_float_P(PSTR("/"),p->endSpeed);
#if USE_OPS==1
      if(!(p->dir & 128) && printer_state.opsMode==2)
        out.println_long_P(PSTR("Reverse at:"),p->opsReverseSteps);
#endif
      out.println_int_P(PSTR("flags:"),p->flags);
      out.println_int_P(PSTR("joinFlags:"),p->joinFlags);
    }
#endif
}
void testnum(float x,char c) {
if (isnan(x)) {
    out.print(c);
    OUT_P_LN("NAN");
    return;
  }

  if (isinf(x)) {
    out.print(c);
     OUT_P_LN("INF");
    return;
  }
}

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

p = last line inserted
last = last element until we check
*/
inline void backwardPlanner(byte p,byte last) {
  if(p==last) return;
  PrintLine *act = &lines[p],*prev;
  float lastJunctionSpeed = act->endSpeed; // Start always with safe speed
  //PREVIOUS_PLANNER_INDEX(last); // Last element is already fixed in start speed
  while(p!=last) {
    PREVIOUS_PLANNER_INDEX(p);
    prev = &lines[p];
#if USE_OPS==1
    // Retraction points are fixed points where the extruder movement stops anyway. Finish the computation for the move and exit.
    if(printer_state.opsMode && printmoveSeen) {
      if((prev->dir & 136)==136 && (act->dir & 136)!=136) {
        if((act->dir & 64)!=0 || act->distance>printer_state.opsMinDistance) { // Switch printing - travel
          act->joinFlags |= FLAG_JOIN_START_RETRACT | FLAG_JOIN_START_FIXED; // enable retract for this point
          prev->joinFlags |= FLAG_JOIN_END_FIXED;
          return;
        } else {
          act->joinFlags |= FLAG_JOIN_NO_RETRACT;
        }
      } else
      if((prev->dir & 136)!=136 && (act->dir & 136)==136) { // Switch travel - print
        prev->joinFlags |= FLAG_JOIN_END_RETRACT | FLAG_JOIN_END_FIXED; // reverse retract for this point
        if(printer_state.opsMode==2) {
          prev->opsReverseSteps = ((long)printer_state.opsPushbackSteps*(long)printer_state.maxExtruderSpeed*TIMER0_PRESCALE)/prev->fullInterval;
          long ponr = prev->stepsRemaining/(1.0+0.01*printer_state.opsMoveAfter);
          if(prev->opsReverseSteps>ponr)
            prev->opsReverseSteps = ponr;
        }
          act->joinFlags |= FLAG_JOIN_START_FIXED; // Wait only with safe speeds!
          return;
      }
    }
#endif
    // Avoid speed calc once crusing in split delta move
#if DRIVE_SYSTEM==3
    if (prev->moveID==act->moveID && lastJunctionSpeed==prev->maxJunctionSpeed) {
      act->startSpeed = prev->endSpeed = lastJunctionSpeed;
      prev->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
      act->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
    }
#endif

    // Switch move-retraction or vice versa start always with save speeds! Keeps extruder from blocking
    if(((prev->dir & 240)!=128) && ((act->dir & 240)==128)) { // switch move - extruder only move
      prev->joinFlags |= FLAG_JOIN_END_FIXED;
      act->joinFlags |= FLAG_JOIN_START_FIXED;
      return;          
    } 
    if(prev->joinFlags & FLAG_JOIN_END_FIXED) { // Nothing to update from here on, happens when path optimize disabled
      act->joinFlags |= FLAG_JOIN_START_FIXED; // Wait only with safe speeds!
      return;
    }
	
    // Avoid speed calcs if we know we can accelerate within the line
    if (act->flags & FLAG_NOMINAL)
      lastJunctionSpeed = act->fullSpeed;
    else
      // If you accelerate from end of move to start what speed to you reach?
      lastJunctionSpeed = sqrt(lastJunctionSpeed*lastJunctionSpeed+act->acceleration); // acceleration is acceleration*distance*2! What can be reached if we try?
      // If that speed is more that the maximum junction speed allowed then ...
      if(lastJunctionSpeed>=prev->maxJunctionSpeed) { // Limit is reached
      // If the previous line's end speed has not been updated to maximum speed then do it now
      if(prev->endSpeed!=prev->maxJunctionSpeed) {
        prev->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
        prev->endSpeed = prev->maxJunctionSpeed; // possibly unneeded???
      }        
      // If actual line start speed has not been updated to maximum speed then do it now
      if(act->startSpeed!=prev->maxJunctionSpeed) {
        act->startSpeed = prev->maxJunctionSpeed; // possibly unneeded???
        act->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
      }
      lastJunctionSpeed = prev->maxJunctionSpeed;     
    } else {
      // Block prev end and act start as calculated speed and recalculate plateau speeds (which could move the speed higher again)
      act->startSpeed = prev->endSpeed = lastJunctionSpeed;
      prev->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
      act->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
    }
    act = prev;
  } // while loop
}

inline void forwardPlanner(byte p) {
  PrintLine *act,*next;
  if(p==lines_write_pos) return;
  byte last = lines_write_pos;
  //NEXT_PLANNER_INDEX(last);
  next = &lines[p];
  float leftspeed = next->startSpeed;

  while(p!=last) { // All except last segment, which has fixed end speed
    act = next;
    NEXT_PLANNER_INDEX(p);
    next = &lines[p];
    if(act->joinFlags & FLAG_JOIN_END_FIXED) {
      leftspeed = act->endSpeed;
      continue; // Nothing to do here
    }
	// Avoid speed calc once crusing in split delta move	
	#if DRIVE_SYSTEM==3
	if (act->moveID == next->moveID && act->endSpeed == act->maxJunctionSpeed) {
		act->startSpeed = leftspeed;
		leftspeed       = act->endSpeed;
	    act->joinFlags  |= FLAG_JOIN_END_FIXED;
        next->joinFlags |= FLAG_JOIN_START_FIXED;
		continue;
	}
	#endif


    float vmax_right;
	// Avoid speed calcs if we know we can accelerate within the line.
	if (act->flags & FLAG_NOMINAL)
	  vmax_right = act->fullSpeed;
	else
      vmax_right = sqrt(leftspeed*leftspeed+act->acceleration); // acceleration is 2*acceleration*distance!, 1000 Ticks
    if(vmax_right>act->endSpeed) { // Could be higher next run?
      act->startSpeed = leftspeed;
      leftspeed       = act->endSpeed;
      if(act->endSpeed==act->maxJunctionSpeed) {// Full speed reached, don't compute again!
        act->joinFlags  |= FLAG_JOIN_END_FIXED;
        next->joinFlags |= FLAG_JOIN_START_FIXED;
      }
      act->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
    } else { // We can accelerate full speed without reaching limit, which is as fast as possible. Fix it!
      act->joinFlags |= FLAG_JOIN_END_FIXED | FLAG_JOIN_START_FIXED;
      act->joinFlags &= ~FLAG_JOIN_STEPPARAMS_COMPUTED; // Needs recomputation
      act->startSpeed = leftspeed;
      act->endSpeed   = next->startSpeed = leftspeed = vmax_right;
      next->joinFlags |= FLAG_JOIN_START_FIXED;
    }
  }
  next->startSpeed = leftspeed; // This is the new segment, wgich is updated anyway, no extra flag needed.
}

/**
This is the path planner.

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

The method is called before lines_count is increased!
*/
void updateTrapezoids(byte p) {
  byte first = p;
  PrintLine *firstLine;
  PrintLine *act = &lines[p];
  BEGIN_INTERRUPT_PROTECTED; 
  byte maxfirst = lines_pos; // first non fixed segment
  if(maxfirst!=p)
    NEXT_PLANNER_INDEX(maxfirst); // don't touch the line printing
  // Now ignore enough segments to gain enough time for path planning
  long timeleft = 0;
  while(timeleft<4500*MOVE_CACHE_SIZE && maxfirst!=p) {
    timeleft+=lines[maxfirst].timeInTicks;
    NEXT_PLANNER_INDEX(maxfirst);
  }
  while(first!=maxfirst && !(lines[first].joinFlags & FLAG_JOIN_END_FIXED)) {
    PREVIOUS_PLANNER_INDEX(first);
  }
  if(first!=p && (lines[first].joinFlags & FLAG_JOIN_END_FIXED)) {
    NEXT_PLANNER_INDEX(first);
  }
  // First is now the new element or the first element with non fixed end speed.
  // anyhow, the start speed of first is fixed
  firstLine = &lines[first];
  firstLine->flags |= FLAG_BLOCKED; // don't let printer touch this or following segments during update
  END_INTERRUPT_PROTECTED; 
  byte previdx = p-1;
  if(previdx>=MOVE_CACHE_SIZE) previdx = MOVE_CACHE_SIZE-1;
  if(lines_count && (lines[previdx].flags & FLAG_WARMUP)==0)
    computeMaxJunctionSpeed(&lines[previdx],act); // Set maximum junction speed if we have a real move before
  else
    act->joinFlags |= FLAG_JOIN_START_FIXED;

  backwardPlanner(p,first);  
  // Reduce speed to reachable speeds
  forwardPlanner(first);
  
  // Update precomputed data
  do {
    updateStepsParameter(&lines[first]);
    lines[first].flags &= ~FLAG_BLOCKED;  // Flying block to release next used segment as early as possible
    NEXT_PLANNER_INDEX(first);
    lines[first].flags |= FLAG_BLOCKED;
  } while(first!=lines_write_pos);
  updateStepsParameter(act);
  act->flags &= ~FLAG_BLOCKED;
}


// ##########################################################################
// ###                         Motion computations                        ###
// ##########################################################################

inline float safeSpeed(PrintLine *p) {
  float safe = min(p->fullSpeed,printer_state.maxJerk*0.5);
#if DRIVE_SYSTEM != 3
  if(p->dir & 64) {
    if(fabs(p->speedZ)>printer_state.maxZJerk*0.5) {
      float safe2 = printer_state.maxZJerk*0.5*p->fullSpeed/fabs(p->speedZ);
      if(safe2<safe) safe = safe2;
    }
  }
#endif
  if(p->dir & 128) {
    if(p->dir & 112) {
      float safe2 = 0.5*current_extruder->maxStartFeedrate*p->fullSpeed/fabs(p->speedE);
      if(safe2<safe) safe = safe2;
    } else {
      safe = 0.5*current_extruder->maxStartFeedrate; // This is a retraction move
    }
  }
  return (safe<p->fullSpeed?safe:p->fullSpeed);
}

/**
Move printer the given number of steps. Puts the move into the queue. Used by e.g. homing commands.
*/
void move_steps(long x,long y,long z,long e,float feedrate,bool waitEnd,bool check_endstop) {
  float saved_feedrate = printer_state.feedrate;
  for(byte i=0; i < 4; i++) {
      printer_state.destinationSteps[i] = printer_state.currentPositionSteps[i];
  }
  printer_state.destinationSteps[0]+=x;
  printer_state.destinationSteps[1]+=y;
  printer_state.destinationSteps[2]+=z;
  printer_state.destinationSteps[3]+=e;
  printer_state.feedrate = feedrate;
#if DRIVE_SYSTEM==3
  split_delta_move(check_endstop,false,false);
#else
  queue_move(check_endstop,false);
#endif
  printer_state.feedrate = saved_feedrate;
  if(waitEnd)
    wait_until_end_of_move();
}

/** Check if move is new. If it is insert some dummy moves to allow the path optimizer to work since it does
not act on the first two moves in the queue. The stepper timer will spot these moves and leave some time for
processing.
*/
byte check_new_move(byte pathOptimize, byte waitExtraLines) {
  if(lines_count==0 && waitRelax==0 && pathOptimize) { // First line after some time - warmup needed
#ifdef DEBUG_OPS
    out.println_P(PSTR("New path"));
#endif
    byte w = 3;
    PrintLine *p = &lines[lines_write_pos];
    while(w) {
      p->flags = FLAG_WARMUP;
      p->joinFlags = FLAG_JOIN_STEPPARAMS_COMPUTED | FLAG_JOIN_END_FIXED | FLAG_JOIN_START_FIXED;
      p->dir = 0;
      p->primaryAxis = w + waitExtraLines;
      p->timeInTicks = p->accelerationPrim = p->facceleration = 10000*(unsigned int)w;
      lines_write_pos++;
      if(lines_write_pos>=MOVE_CACHE_SIZE) lines_write_pos = 0;
BEGIN_INTERRUPT_PROTECTED
      lines_count++;
END_INTERRUPT_PROTECTED
      p = &lines[lines_write_pos];
      w--;
    }
    return 1;
  }
  return 0;
}
void log_long_array(PGM_P ptr,long *arr) {
  out.print_P(ptr);
  for(byte i=0;i<4;i++) {
    out.print(' ');
    out.print(arr[i]);
  }
  out.println();
}
void log_float_array(PGM_P ptr,float *arr) {
  out.print_P(ptr);
  for(byte i=0;i<3;i++)
    out.print_float_P(PSTR(" "),arr[i]);
  out.println_float_P(PSTR(" "),arr[3]);
}
void log_printLine(PrintLine *p) {
  out.println_int_P(PSTR("ID:"),(int)p);
  log_long_array(PSTR("Delta"),p->delta);
  //log_long_array(PSTR("Error"),p->error);
  //out.println_int_P(PSTR("Prim:"),p->primaryAxis);
  out.println_int_P(PSTR("Dir:"),p->dir);
  out.println_int_P(PSTR("Flags:"),p->flags);
  out.println_float_P(PSTR("fullSpeed:"),p->fullSpeed);
  out.println_long_P(PSTR("vMax:"),p->vMax);
  out.println_float_P(PSTR("Acceleration:"),p->acceleration);
  out.println_long_P(PSTR("Acceleration Prim:"),p->accelerationPrim);
  //out.println_long_P(PSTR("Acceleration Timer:"),p->facceleration);
  out.println_long_P(PSTR("Remaining steps:"),p->stepsRemaining);
#ifdef USE_ADVANCE
#ifdef ENABLE_QUADRATIC_ADVANCE
  out.println_long_P(PSTR("advanceFull:"),p->advanceFull>>16);
  out.println_long_P(PSTR("advanceRate:"),p->advanceRate);
#endif
#endif
}


void calculate_move(PrintLine *p,float axis_diff[],byte check_endstops,byte pathOptimize)
{
#if DRIVE_SYSTEM==3
  long axis_interval[5];
#else
  long axis_interval[4];
#endif
  float time_for_move = (float)(F_CPU)*p->distance / printer_state.feedrate; // time is in ticks
  bool critical=false;
  if(lines_count<MOVE_CACHE_LOW && time_for_move<LOW_TICKS_PER_MOVE) { // Limit speed to keep cache full.
    //OUT_P_I("L:",lines_count);
    time_for_move += (3*(LOW_TICKS_PER_MOVE-time_for_move))/(lines_count+1); // Increase time if queue gets empty. Add more time if queue gets smaller.
    //OUT_P_F_LN("Slow ",time_for_move);
    critical=true;
  }
  p->timeInTicks = time_for_move;
  UI_MEDIUM; // do check encoder
  // Compute the solwest allowed interval (ticks/step), so maximum feedrate is not violated
  long limitInterval = time_for_move/p->stepsRemaining; // until not violated by other constraints it is your target speed
  axis_interval[0] = fabs(axis_diff[0])*F_CPU/(max_feedrate[0]*p->stepsRemaining); // mm*ticks/s/(mm/s*steps) = ticks/step
  if(axis_interval[0]>limitInterval) limitInterval = axis_interval[0];
  axis_interval[1] = fabs(axis_diff[1])*F_CPU/(max_feedrate[1]*p->stepsRemaining);
  if(axis_interval[1]>limitInterval) limitInterval = axis_interval[1];
  if(p->dir & 64) { // normally no move in z direction
    axis_interval[2] = fabs((float)axis_diff[2])*(float)F_CPU/(float)(max_feedrate[2]*p->stepsRemaining); // must prevent overflow!
    if(axis_interval[2]>limitInterval) limitInterval = axis_interval[2];
  } else axis_interval[2] = 0;
  axis_interval[3] = fabs(axis_diff[3])*F_CPU/(max_feedrate[3]*p->stepsRemaining);
  if(axis_interval[3]>limitInterval) limitInterval = axis_interval[3];
#if DRIVE_SYSTEM==3
  axis_interval[4] = fabs(axis_diff[4])*F_CPU/(max_feedrate[0]*p->stepsRemaining);
#endif

  p->fullInterval = limitInterval>200 ? limitInterval : 200; // This is our target speed
  // new time at full speed = limitInterval*p->stepsRemaining [ticks]
  time_for_move = (float)limitInterval*(float)p->stepsRemaining; // for large z-distance this overflows with long computation
  float inv_time_s = (float)F_CPU/time_for_move;
  if(p->dir & 16) {
    axis_interval[0] = time_for_move/p->delta[0];
    p->speedX = axis_diff[0]*inv_time_s;
    if(!(p->dir & 1)) p->speedX = -p->speedX;
  } else p->speedX = 0;
  if(p->dir & 32) {
    axis_interval[1] = time_for_move/p->delta[1];
    p->speedY = axis_diff[1]*inv_time_s;
    if(!(p->dir & 2)) p->speedY = -p->speedY;
  } else p->speedY = 0;
  if(p->dir & 64) {
    axis_interval[2] = time_for_move/p->delta[2];
    p->speedZ = axis_diff[2]*inv_time_s;
    if(!(p->dir & 4)) p->speedZ = -p->speedZ;
  } else p->speedZ = 0;
  if(p->dir & 128) {
    axis_interval[3] = time_for_move/p->delta[3];
    p->speedE = axis_diff[3]*inv_time_s;
    if(!(p->dir & 8)) p->speedE = -p->speedE;
  }
#if DRIVE_SYSTEM==3
  axis_interval[4] = time_for_move/p->stepsRemaining;
#endif
  p->fullSpeed = p->distance*inv_time_s;


  //long interval = axis_interval[primary_axis]; // time for every step in ticks with full speed
  byte is_print_move = (p->dir & 136)==136; // are we printing
#if USE_OPS==1
  if(is_print_move) printmoveSeen = 1;
#endif
  //If acceleration is enabled, do some Bresenham calculations depending on which axis will lead it.
  #ifdef RAMP_ACCELERATION

    // slowest time to accelerate from v0 to limitInterval determines used acceleration
    // t = (v_end-v_start)/a
    float slowest_axis_plateau_time_repro = 1e20; // repro to reduce division Unit: 1/s
    for(byte i=0; i < 4 ; i++) {
	// Errors for delta move are initialized in timer
	#if DRIVE_SYSTEM!=3
      p->error[i] = p->delta[p->primaryAxis] >> 1;
	#endif
      if(p->dir & (16<<i)) {
        // v = a * t => t = v/a = F_CPU/(c*a) => 1/t = c*a/F_CPU
        slowest_axis_plateau_time_repro = min(slowest_axis_plateau_time_repro,
               (float)axis_interval[i] * (float)(is_print_move ?  axis_steps_per_sqr_second[i] : axis_travel_steps_per_sqr_second[i])); //  steps/s^2 * step/tick  Ticks/s^2
      }
    }
	// Errors for delta move are initialized in timer (except extruder)
#if DRIVE_SYSTEM==3
    p->error[3] = p->stepsRemaining >> 1;
#endif
    p->invFullSpeed = 1.0/p->fullSpeed;
    p->accelerationPrim = slowest_axis_plateau_time_repro / axis_interval[p->primaryAxis]; // a = v/t = F_CPU/(c*t): Steps/s^2
    //Now we can calculate the new primary axis acceleration, so that the slowest axis max acceleration is not violated
    p->facceleration = 262144.0*(float)p->accelerationPrim/F_CPU; // will overflow without float!
    p->acceleration = 2.0*p->distance*slowest_axis_plateau_time_repro*p->fullSpeed/((float)F_CPU); // mm^2/s^2
    p->startSpeed = p->endSpeed = safeSpeed(p);
	// Can accelerate to full speed within the line
	if (sqrt(p->startSpeed*p->startSpeed+p->acceleration) >= p->fullSpeed)
	  p->flags |= FLAG_NOMINAL;

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

  // Correct integers for fixed point math used in bresenham_step
  if(p->fullInterval<MAX_HALFSTEP_INTERVAL || critical)
    p->halfstep = 0;
  else {
    p->halfstep = 1;
#if DRIVE_SYSTEM==3
    // Error 0-2 are used for the towers and set up in the timer
    p->error[3] = p->stepsRemaining;
#else
    p->error[0] = p->error[1] = p->error[2] = p->error[3] = p->delta[p->primaryAxis];
#endif
  }
#ifdef DEBUG_STEPCOUNT
// Set in delta move calculation
#if DRIVE_SYSTEM!=3
  p->totalStepsRemaining = p->delta[0]+p->delta[1]+p->delta[2];
#endif
#endif
#ifdef DEBUG_QUEUE_MOVE
  if(DEBUG_ECHO) {
    log_printLine(p);
      OUT_P_L_LN("limitInterval:", limitInterval);
      OUT_P_F_LN("Move distance on the XYZ space:", p->distance);
      OUT_P_F_LN("Commanded feedrate:", printer_state.feedrate);
      OUT_P_F_LN("Constant full speed move time:", time_for_move);
      //log_long_array(PSTR("axis_int"),(long*)axis_interval);
      //out.println_float_P(PSTR("Plateau repro:"),slowest_axis_plateau_time_repro);
  }
#endif
  // Make result permanent
  NEXT_PLANNER_INDEX(lines_write_pos);
  if (pathOptimize) waitRelax = 70;
BEGIN_INTERRUPT_PROTECTED
  lines_count++;
END_INTERRUPT_PROTECTED
  DEBUG_MEMORY;
}

#if DRIVE_SYSTEM != 3
/**
  Put a move to the current destination coordinates into the movement cache.
  If the cache is full, the method will wait, until a place gets free. During
  wait communication and temperature control is enabled.
  @param check_endstops Read endstop during move.
*/
void queue_move(byte check_endstops,byte pathOptimize) {
  printer_state.flag0 &= ~PRINTER_FLAG0_STEPPER_DISABLED; // Motor is enabled now
  while(lines_count>=MOVE_CACHE_SIZE) { // wait for a free entry in movement cache
    gcode_read_serial();
    check_periodical();
  }
  byte newPath=check_new_move(pathOptimize, 0);
  PrintLine *p = &lines[lines_write_pos];
  float axis_diff[4]; // Axis movement in mm
  if(check_endstops) p->flags = FLAG_CHECK_ENDSTOPS;
  else p->flags = 0;
  p->joinFlags = 0;
  if(!pathOptimize) p->joinFlags = FLAG_JOIN_END_FIXED;
  p->dir = 0;
#if min_software_endstop_x == true
    if (printer_state.destinationSteps[0] < 0) printer_state.destinationSteps[0] = 0.0;
#endif
#if min_software_endstop_y == true
    if (printer_state.destinationSteps[1] < 0) printer_state.destinationSteps[1] = 0.0;
#endif
#if min_software_endstop_z == true
    if (printer_state.destinationSteps[2] < 0) printer_state.destinationSteps[2] = 0.0;
#endif

#if max_software_endstop_x == true
    if (printer_state.destinationSteps[0] > printer_state.xMaxSteps) printer_state.destinationSteps[0] = printer_state.xMaxSteps;
#endif
#if max_software_endstop_y == true
    if (printer_state.destinationSteps[1] > printer_state.yMaxSteps) printer_state.destinationSteps[1] = printer_state.yMaxSteps;
#endif
#if max_software_endstop_z == true
    if (printer_state.destinationSteps[2] > printer_state.zMaxSteps) printer_state.destinationSteps[2] = printer_state.zMaxSteps;
#endif
  //Find direction
#if DRIVE_SYSTEM==0 || defined(NEW_XY_GANTRY)
  for(byte i=0; i < 4; i++) {
    if((p->delta[i]=printer_state.destinationSteps[i]-printer_state.currentPositionSteps[i])>=0) {
      p->dir |= 1<<i;
    } else {
      p->delta[i] = -p->delta[i];
    }
    if(i==3 && printer_state.extrudeMultiply!=100) {
      p->delta[3]=(long)((p->delta[3]*(float)printer_state.extrudeMultiply)*0.01f); 
      //p->delta[3]=(p->delta[3]*printer_state.extrudeMultiply)/100;
    }
    axis_diff[i] = p->delta[i]*inv_axis_steps_per_unit[i];
    if(p->delta[i]) p->dir |= 16<<i;
    printer_state.currentPositionSteps[i] = printer_state.destinationSteps[i];
  }
  printer_state.filamentPrinted+=axis_diff[3];
#else
  long deltax = printer_state.destinationSteps[0]-printer_state.currentPositionSteps[0];
  long deltay = printer_state.destinationSteps[1]-printer_state.currentPositionSteps[1];
#if DRIVE_SYSTEM==1
  p->delta[2] = printer_state.destinationSteps[2]-printer_state.currentPositionSteps[2];
  p->delta[3] = printer_state.destinationSteps[3]-printer_state.currentPositionSteps[3];
  p->delta[0] = deltax+deltay;
  p->delta[1] = deltax-deltay;
#endif
#if DRIVE_SYSTEM==2
  p->delta[2] = printer_state.destinationSteps[2]-printer_state.currentPositionSteps[2];
  p->delta[3] = printer_state.destinationSteps[3]-printer_state.currentPositionSteps[3];
  p->delta[0] = deltay+deltax;
  p->delta[1] = deltay-deltax;
#endif
  //Find direction
  for(byte i=0; i < 4; i++) {
    if(p->delta[i]>=0) {
      p->dir |= 1<<i;
      axis_diff[i] = p->delta[i]*inv_axis_steps_per_unit[i];
    } else {
      axis_diff[i] = p->delta[i]*inv_axis_steps_per_unit[i];
      p->delta[i] = -p->delta[i];
    }
    if(p->delta[i]) p->dir |= 16<<i;
    printer_state.currentPositionSteps[i] = printer_state.destinationSteps[i];
  }
#endif
  if((p->dir & 240)==0) {
    if(newPath) { // need to delete dummy elements, otherwise commands can get locked.
      lines_count = 0;
      lines_pos = lines_write_pos;
    }
    return; // No steps included
  }
  byte primary_axis;
  float xydist2;
#if USE_OPS==1
  p->opsReverseSteps=0;
#endif
#if ENABLE_BACKLASH_COMPENSATION
  if((p->dir & 112) && ((p->dir & 7)^(printer_state.backlashDir & 7)) & (printer_state.backlashDir >> 3)) { // We need to compensate backlash, add a move
    while(lines_count>=MOVE_CACHE_SIZE-1) { // wait for a second free entry in movement cache
      gcode_read_serial();
      check_periodical();
    }
    byte wpos2 = lines_write_pos+1;
    if(wpos2>=MOVE_CACHE_SIZE) wpos2 = 0;
    PrintLine *p2 = &lines[wpos2];
    memcpy(p2,p,sizeof(PrintLine)); // Move current data to p2
    byte changed = (p->dir & 7)^(printer_state.backlashDir & 7);
    float back_diff[4]; // Axis movement in mm
    back_diff[3] = 0;
    back_diff[0] = (changed & 1 ? (p->dir & 1 ? printer_state.backlashX : -printer_state.backlashX) : 0);
    back_diff[1] = (changed & 2 ? (p->dir & 2 ? printer_state.backlashY : -printer_state.backlashY) : 0);
    back_diff[2] = (changed & 4 ? (p->dir & 4 ? printer_state.backlashZ : -printer_state.backlashZ) : 0);
    p->dir &=7; // x,y and z are already correct
    for(byte i=0; i < 4; i++) {
      float f = back_diff[i]*axis_steps_per_unit[i];
      p->delta[i] = abs((long)f);
      if(p->delta[i]) p->dir |= 16<<i;
    }
    //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
    if(p->delta[1] > p->delta[0] && p->delta[1] > p->delta[2]) p->primaryAxis = 1;
    else if (p->delta[0] > p->delta[2] ) p->primaryAxis = 0;
    else p->primaryAxis = 2;
    p->stepsRemaining = p->delta[p->primaryAxis];
    //Feedrate calc based on XYZ travel distance
    xydist2 = back_diff[0] * back_diff[0] + back_diff[1] * back_diff[1];
    if(p->dir & 64) {
      p->distance = sqrt(xydist2 + back_diff[2] * back_diff[2]);
    } else {
      p->distance = sqrt(xydist2);
    }
    printer_state.backlashDir = (printer_state.backlashDir & 56) | (p2->dir & 7);
    calculate_move(p,back_diff,false,pathOptimize);    
    p = p2; // use saved instance for the real move
  } 
#endif

  //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
  if(p->delta[1] > p->delta[0] && p->delta[1] > p->delta[2] && p->delta[1] > p->delta[3]) primary_axis = 1;
  else if (p->delta[0] > p->delta[2] && p->delta[0] > p->delta[3]) primary_axis = 0;
  else if (p->delta[2] > p->delta[3]) primary_axis = 2;
  else primary_axis = 3;
  p->primaryAxis = primary_axis;
  p->stepsRemaining = p->delta[primary_axis];
  //Feedrate calc based on XYZ travel distance
  // TODO - Simplify since Z will always move
  if(p->dir & 112) {
#if DRIVE_SYSTEM==0 || defined(NEW_XY_GANTRY)
    xydist2 = axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1];
#else
    float dx,dy;
#if DRIVE_SYSTEM==1
    dx = 0.5*(axis_diff[0]+axis_diff[1]);
    dy = axis_diff[0]-dx;
#endif
#if DRIVE_SYSTEM==2
    dy = 0.5*(axis_diff[0]+axis_diff[1]);
    dx = axis_diff[0]-dy;
#endif
    xydist2 = dx*dx+dy*dy;
#endif
    if(p->dir & 64) {
      p->distance = sqrt(xydist2 + axis_diff[2] * axis_diff[2]);
    } else {
      p->distance = sqrt(xydist2);
    }
  }  else if(p->dir & 128)
    p->distance = fabs(axis_diff[3]);
  else {
    return; // no steps to take, we are finished
  }
  calculate_move(p,axis_diff,check_endstops,pathOptimize);
}
#endif

#if DRIVE_SYSTEM==3
#define DEBUG_DELTA_OVERFLOW
/**
  Calculate and cache the delta robot positions of the cartesian move in a line.
  @return The largest delta axis move in a single segment
  @param p The line to examine.
*/
inline long calculate_delta_segments(PrintLine *p, byte softEndstop) {

	long destination_steps[3], destination_delta_steps[3];

	for(byte i=0; i < NUM_AXIS - 1; i++) {
		// Save current position
		destination_steps[i] = printer_state.currentPositionSteps[i];
	}

//	out.println_byte_P(PSTR("Calculate delta segments:"), p->numDeltaSegments);
	p->deltaSegmentReadPos = delta_segment_write_pos;
#ifdef DEBUG_STEPCOUNT
	p->totalStepsRemaining=0;
#endif

	long max_axis_move = 0;
	unsigned int produced_segments = 0;
	for (int s = p->numDeltaSegments; s > 0; s--) {
		for(byte i=0; i < NUM_AXIS - 1; i++)
			destination_steps[i] += (printer_state.destinationSteps[i] - destination_steps[i]) / s;

		// Wait for buffer here
		while(delta_segment_count + produced_segments>=DELTA_CACHE_SIZE) { // wait for a free entry in movement cache
			gcode_read_serial();
			check_periodical();
		}

		DeltaSegment *d = &segments[delta_segment_write_pos];

		// Verify that delta calc has a solution
		if (calculate_delta(destination_steps, destination_delta_steps)) {
			d->dir = 0;
			for(byte i=0; i < NUM_AXIS - 1; i++) {
				if (softEndstop && destination_delta_steps[i] > printer_state.maxDeltaPositionSteps)
					destination_delta_steps[i] = printer_state.maxDeltaPositionSteps;
				long delta = destination_delta_steps[i] - printer_state.currentDeltaPositionSteps[i];
//#ifdef DEBUG_DELTA_CALC
//				out.println_long_P(PSTR("dest:"), destination_delta_steps[i]);
//				out.println_long_P(PSTR("cur:"), printer_state.currentDeltaPositionSteps[i]);
//#endif
				if (delta == 0) {
					d->deltaSteps[i] = 0;
				} else if (delta > 0) {
					d->dir |= 17<<i;
	#ifdef DEBUG_DELTA_OVERFLOW
					if (delta > 65535)
						out.println_long_P(PSTR("Delta overflow:"), delta);
	#endif
					d->deltaSteps[i] = delta;
				} else {
					d->dir |= 16<<i;
	#ifdef DEBUG_DELTA_OVERFLOW
					if (-delta > 65535)
						out.println_long_P(PSTR("Delta overflow:"), delta);
	#endif
					d->deltaSteps[i] = -delta;
				}
	#ifdef DEBUG_STEPCOUNT
				p->totalStepsRemaining += d->deltaSteps[i];
	#endif

				if (max_axis_move < d->deltaSteps[i]) max_axis_move = d->deltaSteps[i];
				printer_state.currentDeltaPositionSteps[i] = destination_delta_steps[i];
			}
		} else {
			// Illegal position - idnore move
			out.println_P(PSTR("Invalid delta coordinate - move ignored"));
			d->dir = 0;
			for(byte i=0; i < NUM_AXIS - 1; i++) {
				d->deltaSteps[i]=0;
			}
		}
		// Move to the next segment
		delta_segment_write_pos++; if (delta_segment_write_pos >= DELTA_CACHE_SIZE) delta_segment_write_pos=0;
		produced_segments++;
	}
	BEGIN_INTERRUPT_PROTECTED
	delta_segment_count+=produced_segments;
	END_INTERRUPT_PROTECTED

	#ifdef DEBUG_STEPCOUNT
//		out.println_long_P(PSTR("totalStepsRemaining:"), p->totalStepsRemaining);
	#endif
	return max_axis_move;
}

/**
  Set delta tower positions
  @param xaxis X tower position.
  @param yaxis Y tower position.
  @param zaxis Z tower position.
*/
inline void set_delta_position(long xaxis, long yaxis, long zaxis) {
	printer_state.currentDeltaPositionSteps[0] = xaxis;
	printer_state.currentDeltaPositionSteps[1] = yaxis;
	printer_state.currentDeltaPositionSteps[2] = zaxis;
}

/**
  Calculate the delta tower position from a cartesian position
  @param cartesianPosSteps Array containing cartesian coordinates.
  @param deltaPosSteps Result array with tower coordinates.
  @returns 1 if cartesian coordinates have a valid delta tower position 0 if not.
*/
byte calculate_delta(long cartesianPosSteps[], long deltaPosSteps[]) {
	long temp;
	long opt = DELTA_DIAGONAL_ROD_STEPS_SQUARED - sq(DELTA_TOWER1_Y_STEPS - cartesianPosSteps[Y_AXIS]);

	if ((temp = opt - sq(DELTA_TOWER1_X_STEPS - cartesianPosSteps[X_AXIS])) >= 0)
		deltaPosSteps[X_AXIS] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
	else
		return 0;

	if ((temp = opt - sq(DELTA_TOWER2_X_STEPS - cartesianPosSteps[X_AXIS])) >= 0)
		deltaPosSteps[Y_AXIS] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
	else
		return 0;

	if ((temp = DELTA_DIAGONAL_ROD_STEPS_SQUARED
		 - sq(DELTA_TOWER3_X_STEPS - cartesianPosSteps[X_AXIS])
		 - sq(DELTA_TOWER3_Y_STEPS - cartesianPosSteps[Y_AXIS])) >= 0)
		deltaPosSteps[Z_AXIS] = sqrt(temp) + cartesianPosSteps[Z_AXIS];
	else
		return 0;

	return 1;
}

inline void calculate_dir_delta(long difference[], byte *dir, long delta[]) {
  *dir = 0;
	//Find direction
	for(byte i=0; i < 4; i++) {
		if(difference[i]>=0) {
			delta[i] = difference[i];
			*dir |= 1<<i;
		} else {
			delta[i] = -difference[i];
	}
		if(delta[i]) *dir |= 16<<i;
	}
	if(printer_state.extrudeMultiply!=100) {
		delta[3]=(long)((delta[3]*(float)printer_state.extrudeMultiply)*0.01f);
	}
}

inline byte calculate_distance(float axis_diff[], byte dir, float *distance) {
  // Calculate distance depending on direction
	if(dir & 112) {
		if(dir & 64) {
			*distance = sqrt(axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1] + axis_diff[2] * axis_diff[2]);
		} else {
			*distance = sqrt(axis_diff[0] * axis_diff[0] + axis_diff[1] * axis_diff[1]);
		}
	} else if(dir & 128)
		*distance = fabs(axis_diff[3]);
	else {
		return 0; // no steps to take, we are finished
	}
  return 1;
}

#ifdef SOFTWARE_LEVELING
void calculate_plane(long factors[], long p1[], long p2[], long p3[]) {
	factors[0] = p1[1] * (p2[2] - p3[2]) + p2[1] * (p3[2] - p1[2]) + p3[1] * (p1[2] - p2[2]);
	factors[1] = p1[2] * (p2[0] - p3[0]) + p2[2] * (p3[0] - p1[0]) + p3[2] * (p1[0] - p2[0]);
	factors[2] = p1[0] * (p2[1] - p3[1]) + p2[0] * (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1]);
	factors[3] = p1[0] * ((p2[1] * p3[2]) - (p3[1] * p2[2])) + p2[0] * ((p3[1] * p1[2]) - (p1[1] * p3[2])) + p3[0] * ((p1[1] * p2[2]) - (p2[1] * p1[2]));
}

float calc_zoffset(long factors[], long pointX, long pointY) {
	return (factors[3] - factors[0] * pointX - factors[1] * pointY) / (float) factors[2];
}
#endif

inline void queue_E_move(long e_diff,byte check_endstops,byte pathOptimize) {
  printer_state.flag0 &= ~PRINTER_FLAG0_STEPPER_DISABLED; // Motor is enabled now
  while(lines_count>=MOVE_CACHE_SIZE) { // wait for a free entry in movement cache
    gcode_read_serial();
    check_periodical();
  }
  byte newPath=check_new_move(pathOptimize, 0);
  PrintLine *p = &lines[lines_write_pos];
  float axis_diff[4]; // Axis movement in mm
  if(check_endstops) p->flags = FLAG_CHECK_ENDSTOPS;
  else p->flags = 0;
  p->joinFlags = 0;
  if(!pathOptimize) p->joinFlags = FLAG_JOIN_END_FIXED;
  p->dir = 0;
  //Find direction
  for(byte i=0; i< 3; i++) {
	p->delta[i] = 0;
	axis_diff[i] = 0;
  }
  axis_diff[3] = e_diff*inv_axis_steps_per_unit[3];
  if (e_diff >= 0) {
	p->delta[3] = e_diff;
	p->dir = 0x88;
  } else {
	p->delta[3] = -e_diff;
	p->dir = 0x80;
  }
  if(printer_state.extrudeMultiply!=100) {
    //p->delta[3]=(p->delta[3]*printer_state.extrudeMultiply)/100;
    p->delta[3]=(long)((p->delta[3]*(float)printer_state.extrudeMultiply)*0.01f);
  }
  printer_state.currentPositionSteps[3] = printer_state.destinationSteps[3];

#if USE_OPS==1
  p->opsReverseSteps=0;
#endif
  p->numDeltaSegments = 0;
  //Define variables that are needed for the Bresenham algorithm. Please note that  Z is not currently included in the Bresenham algorithm.
  p->primaryAxis = 3;
  p->stepsRemaining = p->delta[3];
  p->distance = fabs(axis_diff[3]);
  p->moveID = lastMoveID++;
  calculate_move(p,axis_diff,check_endstops,pathOptimize);
}

/**
  Split a line up into a series of lines with at most MAX_DELTA_SEGMENTS_PER_LINE delta segments.
  @param check_endstops Check endstops during the move.
  @param pathOptimize Run the path optimizer.
  @param delta_step_rate delta step rate in segments per second for the move.
*/
void split_delta_move(byte check_endstops,byte pathOptimize, byte softEndstop) {
    if (softEndstop && printer_state.destinationSteps[2] < 0) printer_state.destinationSteps[2] = 0;
	long difference[NUM_AXIS];
	float axis_diff[5]; // Axis movement in mm. Virtual axis in 4;
	for(byte i=0; i < NUM_AXIS; i++) {
		difference[i] = printer_state.destinationSteps[i] - printer_state.currentPositionSteps[i];
		axis_diff[i] = fabs(difference[i] * inv_axis_steps_per_unit[i]);
	}
    printer_state.filamentPrinted+=axis_diff[3];

#if max_software_endstop_r == true
// TODO - Implement radius checking
// I'm guessing I need the floats to prevent overflow. This is pretty horrible.
// The NaN checking in the delta calculation routine should be enough
//float a = difference[0] * difference[0] + difference[1] * difference[1];
//float b = 2 * (difference[0] * printer_state.currentPositionSteps[0] + difference[1] * printer_state.currentPositionSteps[1]);
//float c = printer_state.currentPositionSteps[0] * printer_state.currentPositionSteps[0] + printer_state.currentPositionSteps[1] * printer_state.currentPositionSteps[1] - r * r;
//float disc = b * b - 4 * a * c;
//if (disc >= 0) {
//    float t = (-b + (float)sqrt(disc)) / (2 * a);
//    printer_state.destinationSteps[0] = (long) printer_state.currentPositionSteps[0] + difference[0] * t;
//    printer_state.destinationSteps[1] = (long) printer_state.currentPositionSteps[1] + difference[1] * t;
//}
#endif

	float save_distance;
	byte save_dir;
	long save_delta[4];
	calculate_dir_delta(difference, &save_dir, save_delta);
	if (!calculate_distance(axis_diff, save_dir, &save_distance))
		return;

	if (!(save_dir & 112)) {
		queue_E_move(difference[3],check_endstops,pathOptimize);
		return;
	}
	
	int segment_count;
	int num_lines;
	int segments_per_line;

	if (save_dir & 48) {
		// Compute number of seconds for move and hence number of segments needed
		float seconds = 100 * save_distance / (printer_state.feedrate * printer_state.feedrateMultiply);
#ifdef DEBUG_SPLIT
		out.println_float_P(PSTR("Seconds: "), seconds);
#endif
		segment_count = max(1, int(((save_dir & 136)==136 ? DELTA_SEGMENTS_PER_SECOND_PRINT : DELTA_SEGMENTS_PER_SECOND_MOVE) * seconds));
		// Now compute the number of lines needed
		num_lines = (segment_count + MAX_DELTA_SEGMENTS_PER_LINE - 1)/MAX_DELTA_SEGMENTS_PER_LINE;
		// There could be some error here but it doesn't matter since the number of segments will just be reduced slightly
		segments_per_line = segment_count / num_lines;
	} else {
		// Optimize pure Z axis move. Since a pure Z axis move is linear all we have to watch out for is unsigned integer overuns in
		// the queued moves;
#ifdef DEBUG_SPLIT
		out.println_long_P(PSTR("Z delta: "), save_delta[2]);
#endif
		segment_count = (save_delta[2] + (unsigned long)65534) / (unsigned long)65535;
		num_lines = (segment_count + MAX_DELTA_SEGMENTS_PER_LINE - 1)/MAX_DELTA_SEGMENTS_PER_LINE;
		segments_per_line = segment_count / num_lines;
	}

	long start_position[4], fractional_steps[4];
	for (byte i = 0; i < 4; i++) {
		start_position[i] = printer_state.currentPositionSteps[i];
	}

#ifdef DEBUG_SPLIT
	out.println_int_P(PSTR("Segments:"), segment_count);
	out.println_int_P(PSTR("Num lines:"), num_lines);
	out.println_int_P(PSTR("segments_per_line:"), segments_per_line);
#endif

	printer_state.flag0 &= ~PRINTER_FLAG0_STEPPER_DISABLED; // Motor is enabled now
	while(lines_count>=MOVE_CACHE_SIZE) { // wait for a free entry in movement cache
		gcode_read_serial();
		check_periodical();
	}

	// Insert dummy moves if necessary
	// Nead to leave at least one slot open for the first split move
	byte newPath=check_new_move(pathOptimize, min(MOVE_CACHE_SIZE-4,num_lines-1));

	for (int line_number=1; line_number < num_lines + 1; line_number++) {
		while(lines_count>=MOVE_CACHE_SIZE) { // wait for a free entry in movement cache
			gcode_read_serial();
			check_periodical();
		}
		PrintLine *p = &lines[lines_write_pos];
		// Downside a comparison per loop. Upside one less distance calculation and simpler code.
		if (num_lines == 1) {
			p->numDeltaSegments = segment_count;
			p->dir = save_dir;
			for (byte i=0; i < 4; i++) {
				p->delta[i] = save_delta[i];
				fractional_steps[i] = difference[i];
			}
			p->distance = save_distance;
		} else {
			for (byte i=0; i < 4; i++) {
				printer_state.destinationSteps[i] = start_position[i] + (difference[i] * line_number / num_lines);
				fractional_steps[i] = printer_state.destinationSteps[i] - printer_state.currentPositionSteps[i];
				axis_diff[i] = fabs(fractional_steps[i]*inv_axis_steps_per_unit[i]);
			}
			calculate_dir_delta(fractional_steps, &p->dir, p->delta);
			calculate_distance(axis_diff, p->dir, &p->distance);
		}

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

		// Only set fixed on last segment
		if (line_number == num_lines && !pathOptimize)
			p->joinFlags = FLAG_JOIN_END_FIXED;

		if(check_endstops)
			p->flags = FLAG_CHECK_ENDSTOPS;
		else
			p->flags = 0;

		p->numDeltaSegments = segments_per_line;

	#if USE_OPS==1
		p->opsReverseSteps=0;
	#endif

		long max_delta_step = calculate_delta_segments(p, softEndstop);

	#ifdef DEBUG_SPLIT
		out.println_long_P(PSTR("Max DS:"), max_delta_step);
	#endif
		long virtual_axis_move = max_delta_step * segments_per_line;
		if (virtual_axis_move == 0 && p->delta[3] == 0) {
			if (num_lines!=1)
				OUT_P_LN("ERROR: No move in delta segment with > 1 segment. This should never happen and may cause a problem!");
			return;  // Line too short in low precision area
		}
		p->primaryAxis = 4; // Virtual axis will lead bresenham step either way
		if (virtual_axis_move > p->delta[3]) { // Is delta move or E axis leading
			p->stepsRemaining = virtual_axis_move;
			axis_diff[4] = virtual_axis_move * inv_axis_steps_per_unit[0]; // Steps/unit same as all the towers
			// Virtual axis steps per segment
			p->numPrimaryStepPerSegment = max_delta_step;
		} else {
			// Round up the E move to get something divisible by segment count which is greater than E move
			p->numPrimaryStepPerSegment = (p->delta[3] + segments_per_line - 1) / segments_per_line;
			p->stepsRemaining = p->numPrimaryStepPerSegment * segments_per_line;
			axis_diff[4] = p->stepsRemaining * inv_axis_steps_per_unit[0];
		}
#ifdef DEBUG_SPLIT
		out.println_long_P(PSTR("Steps Per Segment:"), p->numPrimaryStepPerSegment);
		out.println_long_P(PSTR("Virtual axis step:"), p->stepsRemaining);
#endif

		calculate_move(p,axis_diff,check_endstops,pathOptimize);
		for (byte i=0; i < 4; i++) {
			printer_state.currentPositionSteps[i] += fractional_steps[i];
		}
	}
	lastMoveID++; // Will wrap at 255
}

#endif

#if ARC_SUPPORT
// Arc function taken from grbl
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
// segment is configured in settings.mm_per_arc_segment.  
void mc_arc(float *position, float *target, float *offset, float radius, uint8_t isclockwise)
{      
  //   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
  //   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
  float center_axis0 = position[0] + offset[0];
  float center_axis1 = position[1] + offset[1];
  float linear_travel = 0; //target[axis_linear] - position[axis_linear];
  float extruder_travel = printer_state.destinationSteps[3]-printer_state.currentPositionSteps[3];
  float r_axis0 = -offset[0];  // Radius vector from center to current location
  float r_axis1 = -offset[1];
  float rt_axis0 = target[0] - center_axis0;
  float rt_axis1 = target[1] - center_axis1;
  long xtarget = printer_state.destinationSteps[0];
  long ytarget = printer_state.destinationSteps[1];
  long etarget = printer_state.destinationSteps[3];
  
  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  if (angular_travel < 0) { angular_travel += 2*M_PI; }
  if (isclockwise) { angular_travel -= 2*M_PI; }
  
  float millimeters_of_travel = fabs(angular_travel)*radius; //hypot(angular_travel*radius, fabs(linear_travel));
  if (millimeters_of_travel < 0.001) { return; }
  //uint16_t segments = (radius>=BIG_ARC_RADIUS ? floor(millimeters_of_travel/MM_PER_ARC_SEGMENT_BIG) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT));
  // Increase segment size if printing faster then computation speed allows
  uint16_t segments = (printer_state.feedrate>60 ? floor(millimeters_of_travel/min(MM_PER_ARC_SEGMENT_BIG,printer_state.feedrate*0.01666*MM_PER_ARC_SEGMENT)) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT));
  if(segments == 0) segments = 1;
  /*  
    // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
    // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
    // all segments.
    if (invert_feed_rate) { feed_rate *= segments; }
  */
  float theta_per_segment = angular_travel/segments;
  float linear_per_segment = linear_travel/segments;
  float extruder_per_segment = extruder_travel/segments;
  
  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
         r_T = [cos(phi) -sin(phi);
                sin(phi)  cos(phi] * r ;
     
     For arc generation, the center of the circle is the axis of rotation and the radius vector is 
     defined from the circle center to the initial position. Each line segment is formed by successive
     vector rotations. This requires only two cos() and sin() computations to form the rotation
     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
     all double numbers are single precision on the Arduino. (True double precision will not have
     round off issues for CNC applications.) Single precision error can accumulate to be greater than
     tool precision in some cases. Therefore, arc path correction is implemented. 

     Small angle approximation may be used to reduce computation overhead further. This approximation
     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for 
     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
     issue for CNC machines with the single precision Arduino calculations.
     
     This approximation also allows mc_arc to immediately insert a line segment into the planner 
     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
     This is important when there are successive arc motions. 
  */
  // Vector rotation matrix values
  float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
  float sin_T = theta_per_segment;
  
  float arc_target[4];
  float sin_Ti;
  float cos_Ti;
  float r_axisi;
  uint16_t i;
  int8_t count = 0;

  // Initialize the linear axis
  //arc_target[axis_linear] = position[axis_linear];
  
  // Initialize the extruder axis
  arc_target[3] = printer_state.currentPositionSteps[3];

  for (i = 1; i<segments; i++) 
  { // Increment (segments-1)
    
    if((count & 4) == 0)
    {
       gcode_read_serial();
       check_periodical();
       UI_MEDIUM; // do check encoder
    }

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

    // Update arc_target location
    arc_target[0] = center_axis0 + r_axis0;
    arc_target[1] = center_axis1 + r_axis1;
    //arc_target[axis_linear] += linear_per_segment;
    arc_target[3] += extruder_per_segment;
    
    printer_state.destinationSteps[0] = arc_target[0]*axis_steps_per_unit[0];
    printer_state.destinationSteps[1] = arc_target[1]*axis_steps_per_unit[1];
    printer_state.destinationSteps[3] = arc_target[3];
#if DRIVE_SYSTEM == 3
    split_delta_move(ALWAYS_CHECK_ENDSTOPS, true, true);
#else
    queue_move(ALWAYS_CHECK_ENDSTOPS,true);
#endif
  }
  // Ensure last segment arrives at target location.
  printer_state.destinationSteps[0] = xtarget;
  printer_state.destinationSteps[1] = ytarget;
  printer_state.destinationSteps[3] = etarget;
#if DRIVE_SYSTEM == 3
  split_delta_move(ALWAYS_CHECK_ENDSTOPS, true, true);
#else
  queue_move(ALWAYS_CHECK_ENDSTOPS,true);
#endif
}
#endif