/*
  motion_control.c - high level interface for issuing motion commands
  Part of Grbl

  Copyright (c) 2009-2011 Simen Svale Skogsrud
  Copyright (c) 2011-2013 Sungeun K. Jeon
  Copyright (c) 2011 Jens Geisler
  
  Grbl 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.

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

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
#include <stdlib.h>
#include "settings.h"
#include "config.h"
#include "gcode.h"
#include "motion_control.h"
#include "spindle_control.h"
#include "coolant_control.h"
#include "nuts_bolts.h"
#include "stepper.h"
#include "planner.h"
#include "limits.h"
#include "protocol.h"

// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
// (1 minute)/feed_rate time.
// NOTE: This is the primary gateway to the grbl planner. All line motions, including arc line 
// segments, must pass through this routine before being passed to the planner. The seperation of
// mc_line and plan_buffer_line is done primarily to place non-planner-type functions from being
// in the planner and to let backlash compensation or canned cycle integration simple and direct.
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
  // If enabled, check for soft limit violations. Placed here all line motions are picked up
  // from everywhere in Grbl.
  if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); }    
      
  // If in check gcode mode, prevent motion by blocking planner. Soft limits still work.
  if (sys.state == STATE_CHECK_MODE) { return; }
    
  // TODO: Backlash compensation may be installed here. Only need direction info to track when
  // to insert a backlash line motion(s) before the intended line motion. Requires its own
  // plan_check_full_buffer() and check for system abort loop. Also for position reporting 
  // backlash steps will need to be also tracked. Not sure what the best strategy is for this,
  // i.e. keep the planner independent and do the computations in the status reporting, or let
  // the planner handle the position corrections. The latter may get complicated.
  // TODO: Backlash comp positioning values may need to be kept at a system level, i.e. tracking 
  // true position after a feed hold in the middle of a backlash move. The difficulty is in making 
  // sure that the stepper subsystem and planner are working in sync, and the status report 
  // position also takes this into account.

  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Remain in this loop until there is room in the buffer.
  do {
    protocol_execute_runtime(); // Check for any run-time commands
    if (sys.abort) { return; } // Bail, if system abort.
    if ( plan_check_full_buffer() ) { mc_auto_cycle_start(); } // Auto-cycle start when buffer is full.
    else { break; }
  } while (1);

  plan_buffer_line(target, feed_rate, invert_feed_rate);

  // If idle, indicate to the system there is now a planned block in the buffer ready to cycle 
  // start. Otherwise ignore and continue on.
  if (!sys.state) { sys.state = STATE_QUEUED; }
}


// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
// for vector transformation direction.
// The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance
// of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal
// distance from segment to the circle when the end points both lie on the circle.
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1, 
  uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise)
{      
  float center_axis0 = position[axis_0] + offset[axis_0];
  float center_axis1 = position[axis_1] + offset[axis_1];
  float linear_travel = target[axis_linear] - position[axis_linear];
  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
  float r_axis1 = -offset[axis_1];
  float rt_axis0 = target[axis_0] - center_axis0;
  float rt_axis1 = target[axis_1] - center_axis1;
  
  // 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 (isclockwise) { // Correct atan2 output per direction
    if (angular_travel >= 0) { angular_travel -= 2*M_PI; }
  } else {
    if (angular_travel <= 0) { angular_travel += 2*M_PI; }
  }

  // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to
  // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit
  // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation.
  // Computes: mm_per_arc_segment = sqrt(4*arc_tolerance*(2*radius-arc_tolerance)),
  //           segments = millimeters_of_travel/mm_per_arc_segment
  float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
  uint16_t segments = floor(millimeters_of_travel/
                          sqrt(4*settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
  
  if (segments) { 
    // 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;
    
    /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
       and phi is the angle of rotation. 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. Single precision values can accumulate error greater than tool precision in some
       cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very
       expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute.
  
       Small angle approximation may be used to reduce computation overhead further. A third-order approximation
       (second order sin() has too much error) holds for nearly all CNC applications, except for possibly very
       small radii (~0.5mm). In other words, theta_per_segment would need to be greater than 0.25 rad(14 deg) 
       and N_ARC_CORRECTION would need to be large to cause an appreciable drift error (>5% of radius, for very
       small radii, 5% of 0.5mm is very, very small). N_ARC_CORRECTION~=20 should be more than small enough to 
       correct for numerical drift error. Also decreasing the tolerance will improve the approximation too.
       
       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. 
    */
    // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
    float cos_T = 2.0 - theta_per_segment*theta_per_segment;
    float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
    cos_T *= 0.5;

    float arc_target[N_AXIS];
    float sin_Ti;
    float cos_Ti;
    float r_axisi;
    uint16_t i;
    uint8_t count = 0;
  
    // Initialize the linear axis
    arc_target[axis_linear] = position[axis_linear];
  
    for (i = 1; i<segments; i++) { // Increment (segments-1)
      
      if (count < N_ARC_CORRECTION) {
        // Apply vector rotation matrix. ~40 usec
        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. ~375 usec
        // 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[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
        r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
        count = 0;
      }
  
      // Update arc_target location
      arc_target[axis_0] = center_axis0 + r_axis0;
      arc_target[axis_1] = center_axis1 + r_axis1;
      arc_target[axis_linear] += linear_per_segment;
      mc_line(arc_target, feed_rate, invert_feed_rate);
      
      // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
      if (sys.abort) { return; }
    }
  }
  // Ensure last segment arrives at target location.
  mc_line(target, feed_rate, invert_feed_rate);
}


// Execute dwell in seconds.
void mc_dwell(float seconds) 
{
   uint16_t i = floor(1000/DWELL_TIME_STEP*seconds);
   plan_synchronize();
   delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder
   while (i-- > 0) {
     // NOTE: Check and execute runtime commands during dwell every <= DWELL_TIME_STEP milliseconds.
     protocol_execute_runtime();
     if (sys.abort) { return; }
     _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
   }
}


// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_homing_cycle()
{
  sys.state = STATE_HOMING; // Set system state variable
  limits_disable(); // Disable hard limits pin change register for cycle duration
  plan_reset(); // Reset planner buffer before beginning homing routine.
  st_reset(); // Reset step segment buffer before beginning homing routine.
    
  // -------------------------------------------------------------------------------------
  // Perform homing routine. NOTE: Special motion case. Only system reset works.
  
  // Search to engage all axes limit switches at faster homing seek rate.
  limits_go_home(HOMING_SEARCH_CYCLE_0, true, false, settings.homing_seek_rate);  // Search cycle 0
  #ifdef HOMING_SEARCH_CYCLE_1
    limits_go_home(HOMING_SEARCH_CYCLE_1, true, false, settings.homing_seek_rate);  // Search cycle 1
  #endif
  #ifdef HOMING_SEARCH_CYCLE_2
    limits_go_home(HOMING_SEARCH_CYCLE_2, true, false, settings.homing_seek_rate);  // Search cycle 2
  #endif
    
  // Now in proximity of all limits. Carefully leave and approach switches in multiple cycles
  // to precisely hone in on the machine zero location. Moves at slower homing feed rate.
  int8_t n_cycle = N_HOMING_LOCATE_CYCLE;
  while (n_cycle--) {
    // Leave all switches to release them. After cycles complete, this is machine zero.
    limits_go_home(HOMING_LOCATE_CYCLE, false, true, settings.homing_feed_rate);

    if (n_cycle > 0) {
      // Re-approach all switches to re-engage them.
      limits_go_home(HOMING_LOCATE_CYCLE, true, false, settings.homing_feed_rate);
    }
  }
  // -------------------------------------------------------------------------------------
  
  protocol_execute_runtime(); // Check for reset and set system abort.
  if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.

  // The machine should now be homed and machine limits have been located. By default, 
  // grbl defines machine space as all negative, as do most CNCs. Since limit switches
  // can be on either side of an axes, check and set machine zero appropriately.
  // At the same time, set up pull-off maneuver from axes limit switches that have been homed.
  // This provides some initial clearance off the switches and should also help prevent them 
  // from falsely tripping when hard limits are enabled.
  float pulloff_target[N_AXIS];
  clear_vector_float(pulloff_target); // Zero pulloff target.
  clear_vector_long(sys.position); // Zero current position for now.
  uint8_t idx;
  for (idx=0; idx<N_AXIS; idx++) {
    // Set up pull off targets and machine positions for limit switches homed in the negative
    // direction, rather than the traditional positive. Leave non-homed positions as zero and
    // do not move them.
    // NOTE: settings.max_travel[] is stored as a negative value.
    if (HOMING_LOCATE_CYCLE & bit(idx)) {
      if ( settings.homing_dir_mask & get_direction_mask(idx) ) {
        pulloff_target[idx] = settings.homing_pulloff+settings.max_travel[idx];
        sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
      } else {
        pulloff_target[idx] = -settings.homing_pulloff;
      }
    }
  }
  plan_sync_position(); // Sync planner position to home for pull-off move.

  sys.state = STATE_IDLE; // Set system state to IDLE to complete motion and indicate homed.

  mc_line(pulloff_target, settings.homing_seek_rate, false);
  st_cycle_start(); // Move it. Nothing should be in the buffer except this motion. 
  plan_synchronize(); // Make sure the motion completes.
                      // NOTE: Stepper idle lock resumes normal functionality after cycle.
  
  // The gcode parser position circumvented by the pull-off maneuver, so sync position now.
  gc_sync_position();

  // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
  limits_init();
  // Finished! 
}


// Auto-cycle start is a user setting that automatically begins the cycle when a user enters
// a valid motion command either manually or by a streaming tool. This is intended as a beginners
// feature to help new users to understand g-code. It can be disabled. Otherwise, the normal
// operation of cycle start is manually issuing a cycle start command whenever the user is
// ready and there is a valid motion command in the planner queue.
// NOTE: This function is called from the main loop and mc_line() only and executes when one of
// two conditions exist respectively: There are no more blocks sent (i.e. streaming is finished, 
// single commands), or the planner buffer is full and ready to go.
void mc_auto_cycle_start() { if (sys.auto_start) { st_cycle_start(); } }


// Method to ready the system to reset by setting the runtime reset command and killing any
// active processes in the system. This also checks if a system reset is issued while Grbl
// is in a motion state. If so, kills the steppers and sets the system alarm to flag position
// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by
// runtime abort command and hard limits. So, keep to a minimum.
void mc_reset()
{
  // Only this function can set the system reset. Helps prevent multiple kill calls.
  if (bit_isfalse(sys.execute, EXEC_RESET)) {
    sys.execute |= EXEC_RESET;

    // Kill spindle and coolant.   
    spindle_stop();
    coolant_stop();

    // Kill steppers only if in any motion state, i.e. cycle, feed hold, homing, or jogging
    // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps
    // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
    // violated, by which, all bets are off.
    if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) {
      sys.execute |= EXEC_ALARM; // Execute alarm state.
      st_go_idle(); // Execute alarm force kills steppers. Position likely lost.
    }
  }
}