// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
// entry_factor for each junction. Must be called by planner_recalculate() after 
// updating the blocks.
void TimeEstimateCalculator::recalculate_trapezoids()
{
    Block *current;
    Block *next = NULL;

    for(unsigned int n=0; n<blocks.size(); n--)
    {
        current = next;
        next = &blocks[n];
        if (current)
        {
            // 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_feedrate, next->entry_speed/current->nominal_feedrate);
                current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
            }
        }
    }
    // 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_feedrate, MINIMUM_PLANNER_SPEED/next->nominal_feedrate);
        next->recalculate_flag = false;
    }
}

// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
// entry_factor for each junction. Must be called by planner_recalculate() after 
// updating the blocks.
void planner_recalculate_trapezoids() {
  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) {
      // 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 = 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;
  }
}

// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
// entry_factor for each junction. Must be called by planner_recalculate() after 
// updating the blocks.
void planner_recalculate_trapezoids() {
  uint8_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_factor, next->entry_factor);      
    }
	  block_index = (block_index+1);
	  block_index = block_index % BLOCK_BUFFER_SIZE;
  }
  calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
}

// planner, called whenever a new block was added
// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
// off errors. Only when planned values are converted to stepper rate parameters, these are integers.
inline static void planner_recalculate() {
  //// reverse pass
  // Recalculate entry_speed to be (a) less or equal to vmax_junction and
  // (b) low enough so it can definitely reach the next entry_speed at fixed acceleration.
  int8_t block_index = block_buffer_head;
  block_t *previous = NULL;  // block closer to tail (older)
  block_t *current = NULL;   // block who's entry_speed to be adjusted
  block_t *next = NULL;      // block closer to head (newer)
  while(block_index != block_buffer_tail) {
    block_index = prev_block_index( block_index );
    next = current;
    current = previous;
    previous = &block_buffer[block_index];
    if (current && next) {
      reduce_entry_speed_reverse(current, next);
    }
  } // skip tail/first block

  //// forward pass
  // Recalculate entry_speed to be low enough it can definitely
  // be reached from previous entry_speed at fixed acceleration.
  block_index = block_buffer_tail;
  previous = NULL;  // block closer to tail (older)
  current = NULL;   // block who's entry_speed to be adjusted
  next = NULL;      // block closer to head (newer)
  while(block_index != block_buffer_head) {
    previous = current;
    current = next;
    next = &block_buffer[block_index];
    if (previous && current) {
      reduce_entry_speed_forward(previous, current);
    }
    block_index = next_block_index(block_index);
  }
  if (current && next) {
    reduce_entry_speed_forward(current, next);
  }

  //// recalculate trapeziods for all flagged blocks
  // At this point all blocks have entry_speeds that that can be (a) reached from the prevous
  // entry_speed with the one and only acceleration from our settings and (b) have junction
  // speeds that do not exceed our limits for given direction change.
  // Now we only need to calculate the actual accelerate_until and decelerate_after values.
  block_index = block_buffer_tail;
  current = NULL;
  next = NULL;
  while(block_index != block_buffer_head) {
    current = next;
    next = &block_buffer[block_index];
    if (current) {
      if (current->recalculate_flag || next->recalculate_flag) {
        calculate_trapezoid_for_block( current,
            current->entry_speed/current->nominal_speed,
            next->entry_speed/current->nominal_speed );
        current->recalculate_flag = false;
      }
    }
    block_index = next_block_index( block_index );
  }
  // always recalculate last (newest) block with zero exit speed
  calculate_trapezoid_for_block( next,
    next->entry_speed/next->nominal_speed, ZERO_SPEED/next->nominal_speed );
  next->recalculate_flag = false;
}

//.........这里部分代码省略.........
        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)) );
      }
    }
  }
#endif
  // Start with a safe speed
  float vmax_junction = max_xy_jerk/2; 
  float vmax_junction_factor = 1.0; 
  if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) 
    vmax_junction = min(vmax_junction, max_z_jerk/2);
  if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) 
    vmax_junction = min(vmax_junction, max_e_jerk/2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
    //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) {
      vmax_junction_factor = (max_xy_jerk/jerk);
    } 
    if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
      vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
    } 
    if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
      vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
    } 
    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  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 {
    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;
    }
  }
  /*
    SERIAL_ECHO_START;
   SERIAL_ECHOPGM("advance :");
   SERIAL_ECHO(block->advance/256.0);
   SERIAL_ECHOPGM("advance rate :");
   SERIAL_ECHOLN(block->advance_rate/256.0);
   */
#endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
  safe_speed/block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  memcpy(position, target, sizeof(target)); // position[] = target[]

  planner_recalculate();

  st_wake_up();
}

//.........这里部分代码省略.........
      // 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)) );
        }
      }
    }
  #endif

  // Start with a safe speed
  float vmax_junction = max_xy_jerk / 2;
  float vmax_junction_factor = 1.0;
  float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
  float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
  if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
  if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
          dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
          dz = fabs(csz - previous_speed[Z_AXIS]),
          de = fabs(cse - previous_speed[E_AXIS]),
          jerk = sqrt(dx * dx + dy * dy);

    //    if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
    if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
    if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);

    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  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.
  block->nominal_length_flag = (block->nominal_speed <= v_allowable);
  block->recalculate_flag = true; // Always calculate trapezoid for new block

  // Update previous path unit_vector and nominal speed
  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
  previous_nominal_speed = block->nominal_speed;

  #if ENABLED(ADVANCE)
    // Calculate advance rate
    if (!bse || (!bsx && !bsy && !bsz)) {
      block->advance_rate = 0;
      block->advance = 0;
    }
    else {
      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
      float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
      block->advance = advance;
      block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
    }
    /*
      SERIAL_ECHO_START;
     SERIAL_ECHOPGM("advance :");
     SERIAL_ECHO(block->advance/256.0);
     SERIAL_ECHOPGM("advance rate :");
     SERIAL_ECHOLN(block->advance_rate/256.0);
     */
  #endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];

  planner_recalculate();

  st_wake_up();

} // plan_buffer_line()

void TimeEstimateCalculator::plan(Position newPos, double feedrate)
{
    Block block;
    memset(&block, 0, sizeof(block));
    
    block.maxTravel = 0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        block.delta[n] = newPos[n] - currentPosition[n];
        block.absDelta[n] = fabs(block.delta[n]);
        block.maxTravel = std::max(block.maxTravel, block.absDelta[n]);
    }
    if (block.maxTravel <= 0)
        return;
    if (feedrate < minimumfeedrate)
        feedrate = minimumfeedrate;
    block.distance = sqrtf(square(block.absDelta[0]) + square(block.absDelta[1]) + square(block.absDelta[2]));
    if (block.distance == 0.0)
        block.distance = block.absDelta[3];
    block.nominal_feedrate = feedrate;
    
    Position current_feedrate;
    Position current_abs_feedrate;
    double feedrate_factor = 1.0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        current_feedrate[n] = block.delta[n] * feedrate / block.distance;
        current_abs_feedrate[n] = abs(current_feedrate[n]);
        if (current_abs_feedrate[n] > max_feedrate[n])
            feedrate_factor = std::min(feedrate_factor, max_feedrate[n] / current_abs_feedrate[n]);
    }
    //TODO: XY_FREQUENCY_LIMIT
    
    if(feedrate_factor < 1.0)
    {
        for(unsigned int n=0; n<NUM_AXIS; n++)
        {
            current_feedrate[n] *= feedrate_factor;
            current_abs_feedrate[n] *= feedrate_factor;
        }
        block.nominal_feedrate *= feedrate_factor;
    }
    
    block.acceleration = acceleration;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        if (block.acceleration * (block.absDelta[n] / block.distance) > max_acceleration[n])
            block.acceleration = max_acceleration[n];
    }
    
    double vmax_junction = max_xy_jerk/2; 
    double vmax_junction_factor = 1.0; 
    if(current_abs_feedrate[Z_AXIS] > max_z_jerk/2)
        vmax_junction = std::min(vmax_junction, max_z_jerk/2);
    if(current_abs_feedrate[E_AXIS] > max_e_jerk/2)
        vmax_junction = std::min(vmax_junction, max_e_jerk/2);
    vmax_junction = std::min(vmax_junction, block.nominal_feedrate);
    double safe_speed = vmax_junction;
    
    if ((blocks.size() > 0) && (previous_nominal_feedrate > 0.0001))
    {
        double xy_jerk = sqrt(square(current_feedrate[X_AXIS]-previous_feedrate[X_AXIS])+square(current_feedrate[Y_AXIS]-previous_feedrate[Y_AXIS]));
        vmax_junction = block.nominal_feedrate;
        if (xy_jerk > max_xy_jerk) {
            vmax_junction_factor = (max_xy_jerk/xy_jerk);
        } 
        if(fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS]) > max_z_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_z_jerk/fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS])));
        } 
        if(fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS]) > max_e_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_e_jerk/fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS])));
        } 
        vmax_junction = std::min(previous_nominal_feedrate, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }
    
    block.max_entry_speed = vmax_junction;

    double v_allowable = max_allowable_speed(-block.acceleration, MINIMUM_PLANNER_SPEED, block.distance);
    block.entry_speed = std::min(vmax_junction, v_allowable);
    block.nominal_length_flag = block.nominal_feedrate <= v_allowable;
    block.recalculate_flag = true; // Always calculate trapezoid for new block

    previous_feedrate = current_feedrate;
    previous_nominal_feedrate = block.nominal_feedrate;

    currentPosition = newPos;

    calculate_trapezoid_for_block(&block, block.entry_speed/block.nominal_feedrate, safe_speed/block.nominal_feedrate);
    
    blocks.push_back(block);
}

// 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(double x, double y, double z, double feed_rate,
		      int invert_feed_rate)
{
    // The target position of the tool in absolute steps

    // Calculate target position in absolute steps
    int32_t target[3];
    target[X_AXIS] = lround(x * settings.steps_per_mm[X_AXIS]);
    target[Y_AXIS] = lround(y * settings.steps_per_mm[Y_AXIS]);
    target[Z_AXIS] = lround(z * settings.steps_per_mm[Z_AXIS]);

    // Calculate the buffer head after we push this byte
    int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;
    // If the buffer is full: good! That means we are well ahead of the robot. 
    // Rest here until there is room in the buffer.
    while (block_buffer_tail == next_buffer_head) {
	sleep_mode();
    }
    // Prepare to set up new block
    block_t *block = &block_buffer[block_buffer_head];
    // Number of steps for each axis
    block->steps_x = labs(target[X_AXIS] - position[X_AXIS]);
    block->steps_y = labs(target[Y_AXIS] - position[Y_AXIS]);
    block->steps_z = labs(target[Z_AXIS] - position[Z_AXIS]);
    block->step_event_count =
	max(block->steps_x, max(block->steps_y, block->steps_z));
    // Bail if this is a zero-length block
    if (block->step_event_count == 0) {
	return;
    };

    double delta_x_mm =
	(target[X_AXIS] -
	 position[X_AXIS]) / settings.steps_per_mm[X_AXIS];
    double delta_y_mm =
	(target[Y_AXIS] -
	 position[Y_AXIS]) / settings.steps_per_mm[Y_AXIS];
    double delta_z_mm =
	(target[Z_AXIS] -
	 position[Z_AXIS]) / settings.steps_per_mm[Z_AXIS];
    block->millimeters =
	sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm));


    uint32_t microseconds;
    if (!invert_feed_rate) {
	microseconds = lround((block->millimeters / feed_rate) * 1000000);
    } else {
	microseconds = lround(ONE_MINUTE_OF_MICROSECONDS / feed_rate);
    }

    // Calculate speed in mm/minute for each axis
    double multiplier = 60.0 * 1000000.0 / microseconds;
    block->speed_x = delta_x_mm * multiplier;
    block->speed_y = delta_y_mm * multiplier;
    block->speed_z = delta_z_mm * multiplier;
    block->nominal_speed = block->millimeters * multiplier;
    block->nominal_rate = ceil(block->step_event_count * multiplier);
    block->entry_factor = 0.0;

    // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
    // average travel per step event changes. For a line along one axis the travel per step event
    // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
    // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
    // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
    // specifically for each line to compensate for this phenomenon:
    double travel_per_step = block->millimeters / block->step_event_count;
    block->rate_delta = ceil(((settings.acceleration * 60.0) / (ACCELERATION_TICKS_PER_SECOND)) /	// acceleration mm/sec/sec per acceleration_tick
			     travel_per_step);	// convert to: acceleration steps/min/acceleration_tick    
    if (acceleration_manager_enabled) {
	// compute a preliminary conservative acceleration trapezoid
	double safe_speed_factor = factor_for_safe_speed(block);
	calculate_trapezoid_for_block(block, safe_speed_factor,
				      safe_speed_factor);
    } else {
	block->initial_rate = block->nominal_rate;
	block->final_rate = block->nominal_rate;
	block->accelerate_until = 0;
	block->decelerate_after = block->step_event_count;
	block->rate_delta = 0;
    }

    // Compute direction bits for this block
    block->direction_bits = 0;
    if (target[X_AXIS] < position[X_AXIS]) {
	block->direction_bits |= (1 << X_DIRECTION_BIT);
    }
    if (target[Y_AXIS] < position[Y_AXIS]) {
	block->direction_bits |= (1 << Y_DIRECTION_BIT);
    }
    if (target[Z_AXIS] < position[Z_AXIS]) {
	block->direction_bits |= (1 << Z_DIRECTION_BIT);
    }
    // Move buffer head
    block_buffer_head = next_buffer_head;
    // Update position 
    memcpy(position, target, sizeof(target));	// position[] = target[]
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    // 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)) );
      }
    }
  }
#endif
  // Start with a safe speed
  float vmax_junction = max_xy_jerk / 2;
  float vmax_junction_factor = 1.0; 
  float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
  float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
  if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
  if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
          dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
          dz = fabs(csz - previous_speed[Z_AXIS]),
          de = fabs(cse - previous_speed[E_AXIS]),
          jerk = sqrt(dx * dx + dy * dy);

    //    if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
    if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
    if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);

    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  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.
  block->nominal_length_flag = (block->nominal_speed <= v_allowable); 
  block->recalculate_flag = true; // Always calculate trapezoid for new block

  // Update previous path unit_vector and nominal speed
  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
  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 {
    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] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
    block->advance = advance;
    if(acc_dist == 0) {
      block->advance_rate = 0;
    } 
    else {
      block->advance_rate = advance / (float)acc_dist;
    }
  }
  /*
    SERIAL_ECHO_START;
   SERIAL_ECHOPGM("advance :");
   SERIAL_ECHO(block->advance/256.0);
   SERIAL_ECHOPGM("advance rate :");
   SERIAL_ECHOLN(block->advance_rate/256.0);
   */
#endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];

  planner_recalculate();

  st_wake_up();
}

//.........这里部分代码省略.........
//           next->entry_speed_sqr = entry_speed_sqr;
//           next->recalculate_flag = true;
//         }
//       }
// 
//       // 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_sqr, next->entry_speed_sqr);      
//         current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
//       }
//       
//     current = next;
//     next = &block_buffer[block_index];
//     block_index = next_block_index( block_index );
//   }
//   
//   // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
//   calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
//   next->recalculate_flag = false;
  
  // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but 
  // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
  // find out where in the planner buffer is the a safe place to begin re-planning from.

//   if (block_buffer_head != block_buffer_tail) {   
  float entry_speed_sqr;

  // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
  // tail(first) block to prevent over-writing of the initial entry speed. 
  uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty.
  block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block
  block_t *next;
  if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
  while (block_index != block_buffer_tail) {
    next = current;
    current = &block_buffer[block_index];
    
    // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter
    // the planner nominal speeds at any time. This calc could be done in the override handler, but 
    // this could require an additional variable to be stored to differentiate the programmed nominal
    // speeds, max junction speed, and override speeds/scalar.
    
    // 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_sqr != current->max_entry_speed_sqr) {

      current->entry_speed_sqr = current->max_entry_speed_sqr;
      current->recalculate_flag = true; // Almost always changes. So force recalculate.       

      if (next->entry_speed_sqr < current->max_entry_speed_sqr) {
        // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance      
        entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
        if (entry_speed_sqr < current->max_entry_speed_sqr) {
          current->entry_speed_sqr = entry_speed_sqr;
        }
      } 
    }    
    block_index = prev_block_index( block_index );
  } 

  // Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
  // Also recalculate trapezoids, block by block, as the forward pass completes the plan.
  block_index = next_block_index(block_buffer_tail);
  next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current
  while (block_index != block_buffer_head) {
    current = next;
    next = &block_buffer[block_index];

      // If the current 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 exit speed accordingly. Entry
      // speeds have already been reset, maximized, and reverse planned by reverse planner.
      if (current->entry_speed_sqr < next->entry_speed_sqr) {
        // Compute block exit speed based on the current block speed and distance
        // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
        entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
        
        // If it's less than the stored value, update the exit speed and set recalculate flag.
        if (entry_speed_sqr < next->entry_speed_sqr) {
          next->entry_speed_sqr = entry_speed_sqr;
          next->recalculate_flag = true;
        }
      }

      // 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_sqr, next->entry_speed_sqr);      
        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
      }

    block_index = next_block_index( block_index );
  }
  
  // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
  calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
  next->recalculate_flag = false;
//   }
}

//.........这里部分代码省略.........
  
    // 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/(float)(1.0-sin_theta_d2)) );
            }
        }
    }
#endif
    // Start with a safe speed
    float vmax_junction = max_xy_jerk/2.0; 
    float vmax_junction_factor = 1.0; 

    if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
    if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
    if(G92_reset_previous_speed == 1) {
            vmax_junction = 0.1;
            G92_reset_previous_speed = 0;  
    }

    vmax_junction = fmin(vmax_junction, block->nominal_speed);
    float safe_speed = vmax_junction;

    if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
        float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
        //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
        vmax_junction = block->nominal_speed;
        //    }
        if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
        if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
            vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
        if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
            vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
        vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }
  
  
    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 = fmin(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 = 1;
    else block->nominal_length_flag = 0;
    block->recalculate_flag = 1; // 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;
  
    calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
                                  safe_speed/(float)(block->nominal_speed));
    
    // Move buffer head
    CRITICAL_SECTION_START;
    block_buffer_head = next_buffer_head;
    CRITICAL_SECTION_END;
  
    // Update position
    memcpy(position, target, sizeof(target)); // position[] = target[]

    planner_recalculate();
#ifdef AUTOTEMP
    getHighESpeed();
#endif
    st_wake_up();
}