<commit_before>/********************************************************************
* Description: emccanon.cc
*   Canonical definitions for 3-axis NC application
*
*   Derived from a work by <NAME> & <NAME>
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
********************************************************************/
/*

  Notes:

  Units
  -----
  Values are stored internally as mm and degree units, e.g, program
  offsets, end point, tool length offset.  These are "internal
  units". "External units" are the units used by the EMC motion planner.
  All lengths and units output by the interpreter are converted to
  internal units here, using FROM_PROG_LEN,ANG, and then
  TO_EXT_LEN(),ANG are called to convert these to external units.

  Tool Length Offsets
  -------------------
  The interpreter does not subtract off tool length offsets. It calls
  USE_TOOL_LENGTH_OFFSETS(length), which we record here and apply to
  all appropriate values subsequently.
  */

#include "config.h"
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>		// strncpy()
#include <ctype.h>		// isspace()
#include "emc.hh"		// EMC NML
#include "emc_nml.hh"
#include "canon.hh"
#include "canon_position.hh"		// data type for a machine position
#include "interpl.hh"		// interp_list
#include "emcglb.h"		// TRAJ_MAX_VELOCITY

//#define EMCCANON_DEBUG

//Simple compile-time debug macro
#ifdef EMCCANON_DEBUG
#define canon_debug(...) printf(__VA_ARGS__)
#else
#define canon_debug(...)
#endif

static int debug_velacc = 0;
static double css_maximum, css_numerator; // both always positive
static int spindle_dir = 0;

static const double tiny = 1e-7;
static double xy_rotation = 0.;
static int rotary_unlock_for_traverse = -1;

#ifndef MIN
#define MIN(a,b) ((a)<(b)?(a):(b))
#endif

#ifndef MIN3
#define MIN3(a,b,c) (MIN(MIN((a),(b)),(c)))
#endif

#ifndef MAX
#define MAX(a,b) ((a)>(b)?(a):(b))
#endif

#ifndef MAX3
#define MAX3(a,b,c) (MAX(MAX((a),(b)),(c)))
#endif

#ifndef MAX4
#define MAX4(a,b,c,d) (MAX(MAX((a),(b)),MAX((c),(d))))
#endif

#ifndef MAX9
#define MAX9(a,b,c,d,e,f,g,h,i) (MAX3((MAX3(a,b,c)),(MAX3(d,e,f)),(MAX3(g,h,i))))
#endif

/* macros for converting internal (mm/deg) units to external units */
#define TO_EXT_LEN(mm) ((mm) * GET_EXTERNAL_LENGTH_UNITS())
#define TO_EXT_ANG(deg) ((deg) * GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting external units to internal (mm/deg) units */
#define FROM_EXT_LEN(ext) ((ext) / GET_EXTERNAL_LENGTH_UNITS())
#define FROM_EXT_ANG(ext) ((ext) / GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting internal (mm/deg) units to program units */
#define TO_PROG_LEN(mm) ((mm) / (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define TO_PROG_ANG(deg) (deg)

/* macros for converting program units to internal (mm/deg) units */
#define FROM_PROG_LEN(prog) ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define FROM_PROG_ANG(prog) (prog)

/* Certain axes are periodic.  Hardcode this for now */
#define IS_PERIODIC(axisnum) \
    ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5)

// this doesn't quite work yet: disable
#undef IS_PERIODIC
#define IS_PERIODIC(axisnum) (0)

#define AXIS_PERIOD(axisnum) (IS_PERIODIC(axisnum) ? 360 : 0)

//KLUDGE kinematic data struct (instead of returning a single float value)
//FIXME This should really be refactored into a more general structure, but this
//means tearing up the getStraightXXX functions, which probably means
//converting to canon_position operators
struct VelData {
    double tmax;
    double vel;
    double dtot;
};

struct AccelData{
    double tmax;
    double acc;
    double dtot;
};

static PM_QUATERNION quat(1, 0, 0, 0);

static void flush_segments(void);


/*
  These decls were from the old 3-axis canon.hh, and refer functions
  defined here that are used for convenience but no longer have decls
  in the 6-axis canon.hh. So, we declare them here now.
*/
extern void CANON_ERROR(const char *fmt, ...) __attribute__((format(printf,1,2)));

/*
  Origin offsets, length units, and active plane are all maintained
  here in this file. Controller runs in absolute mode, and does not
  have plane select concept.

  programOrigin is stored in mm always, and converted when set or read.
  When it's applied to positions, convert positions to mm units first
  and then add programOrigin.

  Units are then converted from mm to external units, as reported by
  the GET_EXTERNAL_LENGTH_UNITS() function.
  */
static CANON_POSITION g5xOffset(0.0, 0.0, 0.0, 
                                0.0, 0.0, 0.0,
                                0.0, 0.0, 0.0);
static CANON_POSITION g92Offset(0.0, 0.0, 0.0, 
                                0.0, 0.0, 0.0,
                                0.0, 0.0, 0.0);
static CANON_UNITS lengthUnits = CANON_UNITS_MM;
static CANON_PLANE activePlane = CANON_PLANE_XY;

static int feed_mode = 0;
static int synched = 0;

/* Tool length offset is saved here */
static EmcPose currentToolOffset;

#ifndef D2R
#define D2R(r) ((r)*M_PI/180.0)
#endif

static void rotate(double &x, double &y, double theta) {
    double xx, yy;
    double t = D2R(theta);
    xx = x, yy = y;
    x = xx * cos(t) - yy * sin(t); 
    y = xx * sin(t) + yy * cos(t);
}


/**
 * Implementation of planar rotation for a 3D vector.
 * This is basically a shortcut for "rotate" when the values are stored in a
 * cartesian vector.
 * The use of static "xy_rotation" is ugly here, but is at least consistent.
 */
static void to_rotated(PM_CARTESIAN &vec) {
    rotate(vec.x,vec.y,xy_rotation);
}
#if 0
static void from_rotated(PM_CARTESIAN &vec) {
    rotate(vec.x,vec.y,-xy_rotation);
}
#endif
static void rotate_and_offset(CANON_POSITION & pos) {

    pos += g92Offset;

    rotate(pos.x, pos.y, xy_rotation);

    pos += g5xOffset;

    pos += currentToolOffset;
}

static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) {

    xyz += g92Offset.xyz();

    rotate(xyz.x, xyz.y, xy_rotation);

    xyz += g5xOffset.xyz();

    xyz += PM_CARTESIAN(currentToolOffset.tran.x,
            currentToolOffset.tran.y,
            currentToolOffset.tran.z);
}

static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION pos) {
    CANON_POSITION res;

    res = pos;

    res -= currentToolOffset;
    
    res -= g5xOffset;

    rotate(res.x, res.y, -xy_rotation);

    res -= g92Offset;

    return res;
}

static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {

    x += g92Offset.x;
    y += g92Offset.y;
    z += g92Offset.z;
    a += g92Offset.a;
    b += g92Offset.b;
    c += g92Offset.c;
    u += g92Offset.u;
    v += g92Offset.v;
    w += g92Offset.w;

    rotate(x, y, xy_rotation);

    x += g5xOffset.x;
    y += g5xOffset.y;
    z += g5xOffset.z;
    a += g5xOffset.a;
    b += g5xOffset.b;
    c += g5xOffset.c;
    u += g5xOffset.u;
    v += g5xOffset.v;
    w += g5xOffset.w;

    x += currentToolOffset.tran.x;
    y += currentToolOffset.tran.y;
    z += currentToolOffset.tran.z;
    a += currentToolOffset.a;
    b += currentToolOffset.b;
    c += currentToolOffset.c;
    u += currentToolOffset.u;
    v += currentToolOffset.v;
    w += currentToolOffset.w;
}


static CANON_POSITION unoffset_and_unrotate_pos(const EmcPose pos) {
    CANON_POSITION res(pos);
    return unoffset_and_unrotate_pos(res);
}

// for c in "xyzabcuvw": print "    %s = offset_%s(%s)" % (c,c,c)

static void from_prog(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {
    x = FROM_PROG_LEN(x);
    y = FROM_PROG_LEN(y);
    z = FROM_PROG_LEN(z);
    a = FROM_PROG_ANG(a);
    b = FROM_PROG_ANG(b);
    c = FROM_PROG_ANG(c);
    u = FROM_PROG_LEN(u);
    v = FROM_PROG_LEN(v);
    w = FROM_PROG_LEN(w);
}

static void from_prog(CANON_POSITION &pos) {
    pos.x = FROM_PROG_LEN(pos.x);
    pos.y = FROM_PROG_LEN(pos.y);
    pos.z = FROM_PROG_LEN(pos.z);
    pos.a = FROM_PROG_ANG(pos.a);
    pos.b = FROM_PROG_ANG(pos.b);
    pos.c = FROM_PROG_ANG(pos.c);
    pos.u = FROM_PROG_LEN(pos.u);
    pos.v = FROM_PROG_LEN(pos.v);
    pos.w = FROM_PROG_LEN(pos.w);
}

static void from_prog_len(PM_CARTESIAN &vec) {
    vec.x = FROM_PROG_LEN(vec.x);
    vec.y = FROM_PROG_LEN(vec.y);
    vec.z = FROM_PROG_LEN(vec.z);
}
#if 0
static void to_ext(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {
    x = TO_EXT_LEN(x);
    y = TO_EXT_LEN(y);
    z = TO_EXT_LEN(z);
    a = TO_EXT_ANG(a);
    b = TO_EXT_ANG(b);
    c = TO_EXT_ANG(c);
    u = TO_EXT_LEN(u);
    v = TO_EXT_LEN(v);
    w = TO_EXT_LEN(w);
}

static void to_ext(CANON_POSITION & pos) {
    pos.x=TO_EXT_LEN(pos.x);
    pos.y=TO_EXT_LEN(pos.y);
    pos.z=TO_EXT_LEN(pos.z);
    pos.a=TO_EXT_ANG(pos.a);
    pos.b=TO_EXT_ANG(pos.b);
    pos.c=TO_EXT_ANG(pos.c);
    pos.u=TO_EXT_LEN(pos.u);
    pos.v=TO_EXT_LEN(pos.v);
    pos.w=TO_EXT_LEN(pos.w);
}
#endif

static PM_CARTESIAN to_ext_len(const PM_CARTESIAN & pos) {
    PM_CARTESIAN ret;
    ret.x = TO_EXT_LEN(pos.x);
    ret.y = TO_EXT_LEN(pos.y);
    ret.z = TO_EXT_LEN(pos.z);
    return ret;
}

static EmcPose to_ext_pose(double x, double y, double z, double a, double b, double c, double u, double v, double w) {
    EmcPose result;
    result.tran.x = TO_EXT_LEN(x);
    result.tran.y = TO_EXT_LEN(y);
    result.tran.z = TO_EXT_LEN(z);
    result.a = TO_EXT_ANG(a);
    result.b = TO_EXT_ANG(b);
    result.c = TO_EXT_ANG(c);
    result.u = TO_EXT_LEN(u);
    result.v = TO_EXT_LEN(v);
    result.w = TO_EXT_LEN(w);
    return result;
}

static EmcPose to_ext_pose(const CANON_POSITION & pos) {
    EmcPose result;
    result.tran.x = TO_EXT_LEN(pos.x);
    result.tran.y = TO_EXT_LEN(pos.y);
    result.tran.z = TO_EXT_LEN(pos.z);
    result.a = TO_EXT_ANG(pos.a);
    result.b = TO_EXT_ANG(pos.b);
    result.c = TO_EXT_ANG(pos.c);
    result.u = TO_EXT_LEN(pos.u);
    result.v = TO_EXT_LEN(pos.v);
    result.w = TO_EXT_LEN(pos.w);
    return result;
}

static void to_prog(CANON_POSITION &e) {
    e.x = TO_PROG_LEN(e.x);
    e.y = TO_PROG_LEN(e.y);
    e.z = TO_PROG_LEN(e.z);
    e.a = TO_PROG_ANG(e.a);
    e.b = TO_PROG_ANG(e.b);
    e.c = TO_PROG_ANG(e.c);
    e.u = TO_PROG_LEN(e.u);
    e.v = TO_PROG_LEN(e.v);
    e.w = TO_PROG_LEN(e.w);
}

static int axis_valid(int n) {
    return emcStatus->motion.traj.axis_mask & (1<<n);
}

/*
  canonEndPoint is the last programmed end point, stored in case it's
  needed for subsequent calculations. It's in absolute frame, mm units.

  note that when segments are queued for the naive cam detector that the
  canonEndPoint may not be the last programmed endpoint.  get_last_pos()
  retrieves the xyz position after the last of the queued segments.  these
  are also in absolute frame, mm units.
  */
static CANON_POSITION canonEndPoint;
static void canonUpdateEndPoint(double x, double y, double z, 
                                double a, double b, double c,
                                double u, double v, double w)
{
    canonEndPoint.x = x;
    canonEndPoint.y = y;
    canonEndPoint.z = z;

    canonEndPoint.a = a;
    canonEndPoint.b = b;
    canonEndPoint.c = c;

    canonEndPoint.u = u;
    canonEndPoint.v = v;
    canonEndPoint.w = w;
}

static void canonUpdateEndPoint(const CANON_POSITION & pos)
{
    canonEndPoint = pos;
}

/* External call to update the canon end point.
   Called by emctask during skipping of lines (run-from-line) */
void CANON_UPDATE_END_POINT(double x, double y, double z, 
			    double a, double b, double c, 
			    double u, double v, double w)
{
    canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z),
    			FROM_PROG_ANG(a),FROM_PROG_ANG(b),FROM_PROG_ANG(c),
			FROM_PROG_LEN(u),FROM_PROG_LEN(v),FROM_PROG_LEN(w));
}


/* motion control mode is used to signify blended v. stop-at-end moves.
   Set to 0 (invalid) at start, so first call will send command out */
static CANON_MOTION_MODE canonMotionMode = 0;

/* motion path-following tolerance is used to set the max path-following
   deviation during CANON_CONTINUOUS.
   If this param is 0, then it will behave as emc always did, allowing
   almost any deviation trying to keep speed up. */
static double canonMotionTolerance = 0.0;

static double canonNaivecamTolerance = 0.0;

/* Spindle speed is saved here */
static double spindleSpeed = 0.0; // always positive

/* Prepped tool is saved here */
static int preppedTool = 0;

/* optional program stop */
static bool optional_program_stop = ON; //set enabled by default (previous EMC behaviour)

/* optional block delete */
static bool block_delete = ON; //set enabled by default (previous EMC behaviour)

/*
  Feed rate is saved here; values are in mm/sec or deg/sec.
  It will be initially set in INIT_CANON() below.
*/
static double currentLinearFeedRate = 0.0;
static double currentAngularFeedRate = 0.0;

/* Used to indicate whether the current move is linear, angular, or 
   a combination of both. */
   //AJ says: linear means axes XYZ move (lines or even circles)
   //         angular means axes ABC move
static int cartesian_move = 0;
static int angular_move = 0;

static double toExtVel(double vel) {
    if (cartesian_move && !angular_move) {
	return TO_EXT_LEN(vel);
    } else if (!cartesian_move && angular_move) {
	return TO_EXT_ANG(vel);
    } else if (cartesian_move && angular_move) {
	return TO_EXT_LEN(vel);
    } else { //seems this case was forgotten, neither linear, neither angular move (we are only sending vel)
	return TO_EXT_LEN(vel);
    }	
}

static double toExtAcc(double acc) { return toExtVel(acc); }

static void send_g5x_msg(int index) {
    flush_segments();

    /* append it to interp list so it gets updated at the right time, not at
       read-ahead time */
    EMC_TRAJ_SET_G5X set_g5x_msg;

    set_g5x_msg.g5x_index = index;

    set_g5x_msg.origin = to_ext_pose(g5xOffset);

    if(css_maximum) {
        SET_SPINDLE_SPEED(spindleSpeed);
    }
    interp_list.append(set_g5x_msg);
}

static void send_g92_msg(void) {
    flush_segments();

    /* append it to interp list so it gets updated at the right time, not at
       read-ahead time */
    EMC_TRAJ_SET_G92 set_g92_msg;

    set_g92_msg.origin = to_ext_pose(g92Offset);

    if(css_maximum) {
        SET_SPINDLE_SPEED(spindleSpeed);
    }
    interp_list.append(set_g92_msg);
}

void SET_XY_ROTATION(double t) {
    EMC_TRAJ_SET_ROTATION sr;
    sr.rotation = t;
    interp_list.append(sr);

    xy_rotation = t;
}

void SET_G5X_OFFSET(int index,
                    double x, double y, double z,
                    double a, double b, double c,
                    double u, double v, double w)
{
    CANON_POSITION pos(x,y,z,a,b,c,u,v,w);
    from_prog(pos);

    /* convert to mm units */
    g5xOffset = pos;

    send_g5x_msg(index);
}

void SET_G92_OFFSET(double x, double y, double z,
                    double a, double b, double c,
                    double u, double v, double w) {
    /* convert to mm units */
    CANON_POSITION pos(x,y,z,a,b,c,u,v,w);
    from_prog(pos);

    g92Offset = pos;

    send_g92_msg();
}

void USE_LENGTH_UNITS(CANON_UNITS in_unit)
{
    lengthUnits = in_unit;

    emcStatus->task.programUnits = in_unit;
}

/* Free Space Motion */
void SET_TRAVERSE_RATE(double rate)
{
    // nothing need be done here
}

void SET_FEED_MODE(int mode) {
    flush_segments();
    feed_mode = mode;
    if(feed_mode == 0) STOP_SPEED_FEED_SYNCH();
}

void SET_FEED_RATE(double rate)
{

    if(feed_mode) {
	START_SPEED_FEED_SYNCH(rate, 1);
	currentLinearFeedRate = rate;
    } else {
	/* convert from /min to /sec */
	rate /= 60.0;


	/* convert to traj units (mm & deg) if needed */
	double newLinearFeedRate = FROM_PROG_LEN(rate),
	       newAngularFeedRate = FROM_PROG_ANG(rate);

	if(newLinearFeedRate != currentLinearFeedRate
		|| newAngularFeedRate != currentAngularFeedRate)
	    flush_segments();

	currentLinearFeedRate = newLinearFeedRate;
	currentAngularFeedRate = newAngularFeedRate;
    }
}

void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
{
    // nothing need be done here
}

/**
 * Get the limiting acceleration for a displacement from the current position to the given position.
 * returns a single acceleration that is the minimum of all axis accelerations.
 */
static AccelData getStraightAcceleration(double x, double y, double z,
                               double a, double b, double c,
                               double u, double v, double w)
{
    double dx, dy, dz, du, dv, dw, da, db, dc;
    double tx, ty, tz, tu, tv, tw, ta, tb, tc;
    AccelData out;

    out.acc = 0.0; // if a move to nowhere
    out.tmax = 0.0;
    out.dtot = 0.0;

    // Compute absolute travel distance for each axis:
    dx = fabs(x - canonEndPoint.x);
    dy = fabs(y - canonEndPoint.y);
    dz = fabs(z - canonEndPoint.z);
    da = fabs(a - canonEndPoint.a);
    db = fabs(b - canonEndPoint.b);
    dc = fabs(c - canonEndPoint.c);
    du = fabs(u - canonEndPoint.u);
    dv = fabs(v - canonEndPoint.v);
    dw = fabs(w - canonEndPoint.w);

    if(!axis_valid(0) || dx < tiny) dx = 0.0;
    if(!axis_valid(1) || dy < tiny) dy = 0.0;
    if(!axis_valid(2) || dz < tiny) dz = 0.0;
    if(!axis_valid(3) || da < tiny) da = 0.0;
    if(!axis_valid(4) || db < tiny) db = 0.0;
    if(!axis_valid(5) || dc < tiny) dc = 0.0;
    if(!axis_valid(6) || du < tiny) du = 0.0;
    if(!axis_valid(7) || dv < tiny) dv = 0.0;
    if(!axis_valid(8) || dw < tiny) dw = 0.0;

    if(debug_velacc) 
        printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", 
               dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making.  This is used to determine
    // the units of vel/acc.
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 &&
        du <= 0.0 && dv <= 0.0 && dw <= 0.0) {
	cartesian_move = 0;
    } else {
	cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {
	angular_move = 0;
    } else {
	angular_move = 1;
    }

    // Pure linear move:
    if (cartesian_move && !angular_move) {
	tx = dx? (dx / FROM_EXT_LEN(axis_max_acceleration[0])): 0.0;
	ty = dy? (dy / FROM_EXT_LEN(axis_max_acceleration[1])): 0.0;
	tz = dz? (dz / FROM_EXT_LEN(axis_max_acceleration[2])): 0.0;
	tu = du? (du / FROM_EXT_LEN(axis_max_acceleration[6])): 0.0;
	tv = dv? (dv / FROM_EXT_LEN(axis_max_acceleration[7])): 0.0;
	tw = dw? (dw / FROM_EXT_LEN(axis_max_acceleration[8])): 0.0;
        out.tmax = MAX3(tx, ty ,tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);
        
	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    // Pure angular move:
    else if (!cartesian_move && angular_move) {
	ta = da? (da / FROM_EXT_ANG(axis_max_acceleration[3])): 0.0;
	tb = db? (db / FROM_EXT_ANG(axis_max_acceleration[4])): 0.0;
	tc = dc? (dc / FROM_EXT_ANG(axis_max_acceleration[5])): 0.0;
        out.tmax = MAX3(ta, tb, tc);

	out.dtot = sqrt(da * da + db * db + dc * dc);
	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    // Combination angular and linear move:
    else if (cartesian_move && angular_move) {
	tx = dx? (dx / FROM_EXT_LEN(axis_max_acceleration[0])): 0.0;
	ty = dy? (dy / FROM_EXT_LEN(axis_max_acceleration[1])): 0.0;
	tz = dz? (dz / FROM_EXT_LEN(axis_max_acceleration[2])): 0.0;
	ta = da? (da / FROM_EXT_ANG(axis_max_acceleration[3])): 0.0;
	tb = db? (db / FROM_EXT_ANG(axis_max_acceleration[4])): 0.0;
	tc = dc? (dc / FROM_EXT_ANG(axis_max_acceleration[5])): 0.0;
	tu = du? (du / FROM_EXT_LEN(axis_max_acceleration[6])): 0.0;
	tv = dv? (dv / FROM_EXT_LEN(axis_max_acceleration[7])): 0.0;
	tw = dw? (dw / FROM_EXT_LEN(axis_max_acceleration[8])): 0.0;
        out.tmax = MAX9(tx, ty, tz,
                    ta, tb, tc,
                    tu, tv, tw);

    if(debug_velacc)
        printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", 
               tx, ty, tz, ta, tb, tc, tu, tv, tw);
/*  According to NIST IR6556 Section 2.1.2.5 Paragraph A
    a combnation move is handled like a linear move, except
    that the angular axes are allowed sufficient time to
    complete their motion coordinated with the motion of
    the linear axes.
*/
        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    if(debug_velacc) 
        printf("cartesian %d ang %d acc %g\n", cartesian_move, angular_move, out.acc);
    return out;
}

static AccelData getStraightAcceleration(CANON_POSITION pos)
{

    return getStraightAcceleration(pos.x,
            pos.y,
            pos.z,
            pos.a,
            pos.b,
            pos.c,
            pos.u,
            pos.v,
            pos.w);
}

static VelData getStraightVelocity(double x, double y, double z,
			   double a, double b, double c,
                           double u, double v, double w)
{
    double dx, dy, dz, da, db, dc, du, dv, dw;
    double tx, ty, tz, ta, tb, tc, tu, tv, tw;
    VelData out;

/* If we get a move to nowhere (!cartesian_move && !angular_move)
   we might as well go there at the currentLinearFeedRate...
*/
    out.vel = currentLinearFeedRate;
    out.tmax = 0;
    out.dtot = 0;

    // Compute absolute travel distance for each axis:
    dx = fabs(x - canonEndPoint.x);
    dy = fabs(y - canonEndPoint.y);
    dz = fabs(z - canonEndPoint.z);
    da = fabs(a - canonEndPoint.a);
    db = fabs(b - canonEndPoint.b);
    dc = fabs(c - canonEndPoint.c);
    du = fabs(u - canonEndPoint.u);
    dv = fabs(v - canonEndPoint.v);
    dw = fabs(w - canonEndPoint.w);

    if(!axis_valid(0) || dx < tiny) dx = 0.0;
    if(!axis_valid(1) || dy < tiny) dy = 0.0;
    if(!axis_valid(2) || dz < tiny) dz = 0.0;
    if(!axis_valid(3) || da < tiny) da = 0.0;
    if(!axis_valid(4) || db < tiny) db = 0.0;
    if(!axis_valid(5) || dc < tiny) dc = 0.0;
    if(!axis_valid(6) || du < tiny) du = 0.0;
    if(!axis_valid(7) || dv < tiny) dv = 0.0;
    if(!axis_valid(8) || dw < tiny) dw = 0.0;

    if(debug_velacc) 
        printf("getStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n",
               dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making:
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 &&
        du <= 0.0 && dv <= 0.0 && dw <= 0.0) {
	cartesian_move = 0;
    } else {
	cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {
	angular_move = 0;
    } else {
	angular_move = 1;
    }

    // Pure linear move:
    if (cartesian_move && !angular_move) {
        tx = dx? fabs(dx / FROM_EXT_LEN(axis_max_velocity[0])): 0.0;
        ty = dy? fabs(dy / FROM_EXT_LEN(axis_max_velocity[1])): 0.0;
        tz = dz? fabs(dz / FROM_EXT_LEN(axis_max_velocity[2])): 0.0;
        tu = du? fabs(du / FROM_EXT_LEN(axis_max_velocity[6])): 0.0;
        tv = dv? fabs(dv / FROM_EXT_LEN(axis_max_velocity[7])): 0.0;
        tw = dw? fabs(dw / FROM_EXT_LEN(axis_max_velocity[8])): 0.0;
        out.tmax = MAX3(tx, ty ,tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0) {
            out.vel = currentLinearFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Pure angular move:
    else if (!cartesian_move && angular_move) {
        ta = da? fabs(da / FROM_EXT_ANG(axis_max_velocity[3])):0.0;
        tb = db? fabs(db / FROM_EXT_ANG(axis_max_velocity[4])):0.0;
        tc = dc? fabs(dc / FROM_EXT_ANG(axis_max_velocity[5])):0.0;
        out.tmax = MAX3(ta, tb, tc);

        out.dtot = sqrt(da * da + db * db + dc * dc);
        if (out.tmax <= 0.0) {
            out.vel = currentAngularFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Combination angular and linear move:
    else if (cartesian_move && angular_move) {
        tx = dx? fabs(dx / FROM_EXT_LEN(axis_max_velocity[0])): 0.0;
        ty = dy? fabs(dy / FROM_EXT_LEN(axis_max_velocity[1])): 0.0;
        tz = dz? fabs(dz / FROM_EXT_LEN(axis_max_velocity[2])): 0.0;
        ta = da? fabs(da / FROM_EXT_ANG(axis_max_velocity[3])): 0.0;
        tb = db? fabs(db / FROM_EXT_ANG(axis_max_velocity[4])): 0.0;
        tc = dc? fabs(dc / FROM_EXT_ANG(axis_max_velocity[5])): 0.0;
        tu = du? fabs(du / FROM_EXT_LEN(axis_max_velocity[6])): 0.0;
        tv = dv? fabs(dv / FROM_EXT_LEN(axis_max_velocity[7])): 0.0;
        tw = dw? fabs(dw / FROM_EXT_LEN(axis_max_velocity[8])): 0.0;
        out.tmax = MAX9(tx, ty, tz,
                ta, tb, tc,
                tu, tv, tw);

        if(debug_velacc)
            printf("getStraightVelocity times tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
                    tx, ty, tz, ta, tb, tc, tu, tv, tw);

        /*  According to NIST IR6556 Section 2.1.2.5 Paragraph A
            a combnation move is handled like a linear move, except
            that the angular axes are allowed sufficient time to
            complete their motion coordinated with the motion of
            the linear axes.
            */
        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0) {
            out.vel = currentLinearFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    if(debug_velacc) 
        printf("cartesian %d ang %d vel %g\n", cartesian_move, angular_move, out.vel);
    return out;
}

static VelData getStraightVelocity(CANON_POSITION pos)
{

    return getStraightVelocity(pos.x,
            pos.y,
            pos.z,
            pos.a,
            pos.b,
            pos.c,
            pos.u,
            pos.v,
            pos.w);
}

#include <vector>
struct pt { double x, y, z, a, b, c, u, v, w; int line_no;};

static std::vector<struct pt> chained_points;

static void flush_segments(void) {
    if(chained_points.empty()) return;

    struct pt &pos = chained_points.back();

    double x = pos.x, y = pos.y, z = pos.z;
    double a = pos.a, b = pos.b, c = pos.c;
    double u = pos.u, v = pos.v, w = pos.w;
    
    int line_no = pos.line_no;

#ifdef SHOW_JOINED_SEGMENTS
    for(unsigned int i=0; i != chained_points.size(); i++) { printf("."); }
    printf("\n");
#endif

    VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    double vel = linedata.vel;

    if (cartesian_move && !angular_move) {
        if (vel > currentLinearFeedRate) {
            vel = currentLinearFeedRate;
        }
    } else if (!cartesian_move && angular_move) {
        if (vel > currentAngularFeedRate) {
            vel = currentAngularFeedRate;
        }
    } else if (cartesian_move && angular_move) {
        if (vel > currentLinearFeedRate) {
            vel = currentLinearFeedRate;
        }
    }


    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = feed_mode;

    // now x, y, z, and b are in absolute mm or degree units
    linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
    linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
    linearMoveMsg.end.tran.z = TO_EXT_LEN(z);

    linearMoveMsg.end.u = TO_EXT_LEN(u);
    linearMoveMsg.end.v = TO_EXT_LEN(v);
    linearMoveMsg.end.w = TO_EXT_LEN(w);

    // fill in the orientation
    linearMoveMsg.end.a = TO_EXT_ANG(a);
    linearMoveMsg.end.b = TO_EXT_ANG(b);
    linearMoveMsg.end.c = TO_EXT_ANG(c);

    linearMoveMsg.vel = toExtVel(vel);
    linearMoveMsg.ini_maxvel = toExtVel(linedata.vel);
    AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    double acc = lineaccdata.acc;
    linearMoveMsg.acc = toExtAcc(acc);

    linearMoveMsg.type = EMC_MOTION_TYPE_FEED;
    linearMoveMsg.indexrotary = -1;
    if ((vel && acc) || synched) {
        interp_list.set_line_number(line_no);
        interp_list.append(linearMoveMsg);
    }
    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);

    chained_points.clear();
}

static void get_last_pos(double &lx, double &ly, double &lz) {
    if(chained_points.empty()) {
        lx = canonEndPoint.x;
        ly = canonEndPoint.y;
        lz = canonEndPoint.z;
    } else {
        struct pt &pos = chained_points.back();
        lx = pos.x;
        ly = pos.y;
        lz = pos.z;
    }
}

static bool
linkable(double x, double y, double z, 
         double a, double b, double c, 
         double u, double v, double w) {
    struct pt &pos = chained_points.back();
    if(canonMotionMode != CANON_CONTINUOUS || canonNaivecamTolerance == 0)
        return false;
    //FIXME make this length controlled elsewhere?
    if(chained_points.size() > 100) return false;

    //If ABCUVW motion, then the tangent calculation fails?
    // TODO is there a fundamental reason that we can't handle 9D motion here?
    if(a != pos.a) return false;
    if(b != pos.b) return false;
    if(c != pos.c) return false;
    if(u != pos.u) return false;
    if(v != pos.v) return false;
    if(w != pos.w) return false;

    if(x==canonEndPoint.x && y==canonEndPoint.y && z==canonEndPoint.z) return false;
    
    for(std::vector<struct pt>::iterator it = chained_points.begin();
            it != chained_points.end(); it++) {
        PM_CARTESIAN M(x-canonEndPoint.x, y-canonEndPoint.y, z-canonEndPoint.z),
                     B(canonEndPoint.x, canonEndPoint.y, canonEndPoint.z),
                     P(it->x, it->y, it->z);
        double t0 = dot(M, P-B) / dot(M, M);
        if(t0 < 0) t0 = 0;
        if(t0 > 1) t0 = 1;

        double D = mag(P - (B + t0 * M));
        if(D > canonNaivecamTolerance) return false;
    }
    return true;
}

static void
see_segment(int line_number,
	    double x, double y, double z, 
            double a, double b, double c,
            double u, double v, double w) {
    bool changed_abc = (a != canonEndPoint.a)
        || (b != canonEndPoint.b)
        || (c != canonEndPoint.c);

    bool changed_uvw = (u != canonEndPoint.u)
        || (v != canonEndPoint.v)
        || (w != canonEndPoint.w);

    if(!chained_points.empty() && !linkable(x, y, z, a, b, c, u, v, w)) {
        flush_segments();
    }
    pt pos = {x, y, z, a, b, c, u, v, w, line_number};
    chained_points.push_back(pos);
    if(changed_abc || changed_uvw) {
        flush_segments();
    }
}

void FINISH() {
    flush_segments();
}

void STRAIGHT_TRAVERSE(int line_number,
                       double x, double y, double z,
		       double a, double b, double c,
                       double u, double v, double w)
{
    double vel, acc;

    flush_segments();

    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;

    linearMoveMsg.feed_mode = 0;
    if (rotary_unlock_for_traverse != -1)
        linearMoveMsg.type = EMC_MOTION_TYPE_INDEXROTARY;
    else
        linearMoveMsg.type = EMC_MOTION_TYPE_TRAVERSE;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);

    VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);

    vel = veldata.vel;
    acc = accdata.acc;

    linearMoveMsg.end = to_ext_pose(x,y,z,a,b,c,u,v,w);
    linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel);
    linearMoveMsg.acc = toExtAcc(acc);
    linearMoveMsg.indexrotary = rotary_unlock_for_traverse;

    int old_feed_mode = feed_mode;
    if(feed_mode)
	STOP_SPEED_FEED_SYNCH();

    if(vel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(linearMoveMsg);
    }

    if(old_feed_mode)
	START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1);

    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

void STRAIGHT_FEED(int line_number,
                   double x, double y, double z, 
                   double a, double b, double c,
                   double u, double v, double w)
{
    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = feed_mode;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);
    see_segment(line_number, x, y, z, a, b, c, u, v, w);
}


void RIGID_TAP(int line_number, double x, double y, double z)
{
    double ini_maxvel,acc;
    EMC_TRAJ_RIGID_TAP rigidTapMsg;
    double unused=0;

    from_prog(x,y,z,unused,unused,unused,unused,unused,unused);
    rotate_and_offset_pos(x,y,z,unused,unused,unused,unused,unused,unused);

    VelData veldata = getStraightVelocity(x, y, z,
                              canonEndPoint.a, canonEndPoint.b, canonEndPoint.c, 
                              canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);

    ini_maxvel = veldata.vel;
    
    AccelData accdata = getStraightAcceleration(x, y, z,
                                  canonEndPoint.a, canonEndPoint.b, canonEndPoint.c,
                                  canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);
    acc = accdata.acc;
    
    rigidTapMsg.pos = to_ext_pose(x,y,z,
                                 canonEndPoint.a, canonEndPoint.b, canonEndPoint.c,
                                 canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);

    rigidTapMsg.vel = toExtVel(ini_maxvel);
    rigidTapMsg.ini_maxvel = toExtVel(ini_maxvel);
    rigidTapMsg.acc = toExtAcc(acc);

    flush_segments();

    if(ini_maxvel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(rigidTapMsg);
    }

    // don't move the endpoint because after this move, we are back where we started
}


/*
  STRAIGHT_PROBE is exactly the same as STRAIGHT_FEED, except that it
  uses a probe message instead of a linear move message.
*/

void STRAIGHT_PROBE(int line_number,
                    double x, double y, double z, 
                    double a, double b, double c,
                    double u, double v, double w,
                    unsigned char probe_type)
{
    double ini_maxvel, vel, acc;
    EMC_TRAJ_PROBE probeMsg;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);

    flush_segments();

    VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    ini_maxvel = vel = veldata.vel;

    if (cartesian_move && !angular_move) {
	if (vel > currentLinearFeedRate) {
	    vel = currentLinearFeedRate;
	}
    } else if (!cartesian_move && angular_move) {
	if (vel > currentAngularFeedRate) {
	    vel = currentAngularFeedRate;
	}
    } else if (cartesian_move && angular_move) {
	if (vel > currentLinearFeedRate) {
	    vel = currentLinearFeedRate;
	}
    }

    AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    acc = accdata.acc;

    probeMsg.vel = toExtVel(vel);
    probeMsg.ini_maxvel = toExtVel(ini_maxvel);
    probeMsg.acc = toExtAcc(acc);

    probeMsg.type = EMC_MOTION_TYPE_PROBING;
    probeMsg.probe_type = probe_type;

    probeMsg.pos = to_ext_pose(x,y,z,a,b,c,u,v,w);

    if(vel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(probeMsg);
    }
    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

/* Machining Attributes */

void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance)
{
    EMC_TRAJ_SET_TERM_COND setTermCondMsg;

    flush_segments();

    canonMotionMode = mode;
    canon_debug("canonMotionMode = %d\n", mode);
    canonMotionTolerance =  FROM_PROG_LEN(tolerance);

    switch (mode) {
    case CANON_CONTINUOUS:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND;
        setTermCondMsg.tolerance = TO_EXT_LEN(canonMotionTolerance);
        break;
    case CANON_EXACT_PATH:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_EXACT;
        break;

    case CANON_EXACT_STOP:
    default:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP;
        break;
    }

    interp_list.append(setTermCondMsg);
}

void SET_NAIVECAM_TOLERANCE(double tolerance)
{
    canonNaivecamTolerance =  FROM_PROG_LEN(tolerance);
}

void SELECT_PLANE(CANON_PLANE in_plane)
{
    activePlane = in_plane;
}

void SET_CUTTER_RADIUS_COMPENSATION(double radius)
{
    // nothing need be done here
}

void START_CUTTER_RADIUS_COMPENSATION(int side)
{
    // nothing need be done here
}

void STOP_CUTTER_RADIUS_COMPENSATION()
{
    // nothing need be done here
}



void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode)
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution));
    spindlesyncMsg.velocity_mode = velocity_mode;
    interp_list.append(spindlesyncMsg);
    synched = 1;
}

void STOP_SPEED_FEED_SYNCH()
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = 0.0;
    spindlesyncMsg.velocity_mode = false;
    interp_list.append(spindlesyncMsg);
    synched = 0;
}

/* Machining Functions */
static double chord_deviation(double sx, double sy, double ex, double ey, double cx, double cy, int rotation, double &mx, double &my) {
    double th1 = atan2(sy-cy, sx-cx),
           th2 = atan2(ey-cy, ex-cx),
           r = hypot(sy-cy, sx-cx),
           dth = th2 - th1;

    if(rotation < 0) {
        if(dth >= -1e-5) th2 -= 2*M_PI;
        // in the edge case where atan2 gives you -pi and pi, a second iteration is needed
        // to get these in the right order
        dth = th2 - th1;
        if(dth >= -1e-5) th2 -= 2*M_PI;
    } else {
        if(dth <= 1e-5) th2 += 2*M_PI;
        dth = th2 - th1;
        if(dth <= 1e-5) th2 += 2*M_PI;
    }

    double included = fabs(th2 - th1);
    double mid = (th2 + th1) / 2;
    mx = cx + r * cos(mid);
    my = cy + r * sin(mid);
    double dev = r * (1 - cos(included/2));
    return dev;
}

/* Spline and NURBS additional functions; */

static double max(double a, double b) {
    if(a < b) return b;
    return a;
}
static void unit(double *x, double *y) {
    double h = hypot(*x, *y);
    if(h != 0) { *x/=h; *y/=h; }
}

static void
arc(int lineno, double x0, double y0, double x1, double y1, double dx, double dy) {
    double small = 0.000001;
    double x = x1-x0, y=y1-y0;
    double den = 2 * (y*dx - x*dy);
    CANON_POSITION p = unoffset_and_unrotate_pos(canonEndPoint);
    to_prog(p);
    if (fabs(den) > small) {
        double r = -(x*x+y*y)/den;
        double i = dy*r, j = -dx*r;
        double cx = x0+i, cy=y0+j;
        ARC_FEED(lineno, x1, y1, cx, cy, r<0 ? 1 : -1,
                 p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    } else { 
        STRAIGHT_FEED(lineno, x1, y1, p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    }
}

static int
biarc(int lineno, double p0x, double p0y, double tsx, double tsy,
      double p4x, double p4y, double tex, double tey, double r=1.0) {
    unit(&tsx, &tsy);
    unit(&tex, &tey);

    double vx = p0x - p4x, vy = p0y - p4y;
    double c = vx*vx + vy*vy;
    double b = 2 * (vx * (r*tsx + tex) + vy * (r*tsy + tey));
    double a = 2 * r * (tsx * tex + tsy * tey - 1);

    double discr = b*b - 4*a*c;
    if(discr < 0) return 0;

    double disq = sqrt(discr);
    double beta1 = (-b-disq) / 2 / a;
    double beta2 = (-b+disq) / 2 / a;

    if(beta1 > 0 && beta2 > 0)
      return 0;
    double beta = max(beta1, beta2);
    double alpha = beta * r;
    double ab = alpha + beta;
    double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy,
         p3x = p4x - beta * tex, p3y = p4y - beta * tey,
         p2x = (p1x*beta + p3x*alpha) / ab,
         p2y = (p1y*beta + p3y*alpha) / ab;
    double tmx = p3x-p2x, tmy = p3y-p2y;
    unit(&tmx, &tmy);

    arc(lineno, p0x, p0y, p2x, p2y, tsx, tsy);
    arc(lineno, p2x, p2y, p4x, p4y, tmx, tmy);
    return 1;
}


/* Canon calls */

void NURBS_FEED(int lineno, std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k) {
    flush_segments();

    unsigned int n = nurbs_control_points.size() - 1;
    double umax = n - k + 2;
    unsigned int div = nurbs_control_points.size()*4;
    std::vector<unsigned int> knot_vector = knot_vector_creator(n, k);	
    PLANE_POINT P0, P0T, P1, P1T;

    P0 = nurbs_point(0,k,nurbs_control_points,knot_vector);
    P0T = nurbs_tangent(0, k, nurbs_control_points, knot_vector);

    for(unsigned int i=1; i<=div; i++) {
	double u = umax * i / div;
        P1 = nurbs_point(u,k,nurbs_control_points,knot_vector);
	P1T = nurbs_tangent(u,k,nurbs_control_points,knot_vector);
        biarc(lineno, P0.X,P0.Y, P0T.X,P0T.Y, P1.X,P1.Y, P1T.X,P1T.Y);
        P0 = P1;
        P0T = P1T;
    }
    knot_vector.clear();
}


/**
 * Simple circular shift function for PM_CARTESIAN type.
 * Cycle around axes without changing the individual values. A circshift of -1
 * makes the X value become the new Y, Y become the Z, and Z become the new X.
 */
static PM_CARTESIAN circshift(PM_CARTESIAN & vec, int steps)
{
    int X=0,Y=1,Z=2;

    int s = 3;
    // Use mod to cycle indices around by steps
    X = (X + steps + s) % s;
    Y = (Y + steps + s) % s;
    Z = (Z + steps + s) % s;
    return PM_CARTESIAN(vec[X],vec[Y],vec[Z]);
}

#if 0
static CANON_POSITION get_axis_max_velocity()
{
    CANON_POSITION maxvel;
    maxvel.x = axis_valid(0) ? FROM_EXT_LEN(axis_max_velocity[0]) : 0.0;
    maxvel.y = axis_valid(1) ? FROM_EXT_LEN(axis_max_velocity[1]) : 0.0;
    maxvel.z = axis_valid(2) ? FROM_EXT_LEN(axis_max_velocity[2]) : 0.0;

    maxvel.a = axis_valid(3) ? FROM_EXT_ANG(axis_max_velocity[3]) : 0.0;
    maxvel.b = axis_valid(4) ? FROM_EXT_ANG(axis_max_velocity[4]) : 0.0;
    maxvel.c = axis_valid(5) ? FROM_EXT_ANG(axis_max_velocity[5]) : 0.0;

    maxvel.u = axis_valid(6) ? FROM_EXT_LEN(axis_max_velocity[6]) : 0.0;
    maxvel.v = axis_valid(7) ? FROM_EXT_LEN(axis_max_velocity[7]) : 0.0;
    maxvel.w = axis_valid(8) ? FROM_EXT_LEN(axis_max_velocity[8]) : 0.0;
    return maxvel;
}

static CANON_POSITION get_axis_max_acceleration()
{
    CANON_POSITION maxacc;
    maxacc.x = axis_valid(0) ? FROM_EXT_LEN(axis_max_acceleration[0]) : 0.0;
    maxacc.y = axis_valid(1) ? FROM_EXT_LEN(axis_max_acceleration[1]) : 0.0;
    maxacc.z = axis_valid(2) ? FROM_EXT_LEN(axis_max_acceleration[2]) : 0.0;

    maxacc.a = axis_valid(3) ? FROM_EXT_ANG(axis_max_acceleration[3]) : 0.0;
    maxacc.b = axis_valid(4) ? FROM_EXT_ANG(axis_max_acceleration[4]) : 0.0;
    maxacc.c = axis_valid(5) ? FROM_EXT_ANG(axis_max_acceleration[5]) : 0.0;

    maxacc.u = axis_valid(6) ? FROM_EXT_LEN(axis_max_acceleration[6]) : 0.0;
    maxacc.v = axis_valid(7) ? FROM_EXT_LEN(axis_max_acceleration[7]) : 0.0;
    maxacc.w = axis_valid(8) ? FROM_EXT_LEN(axis_max_acceleration[8]) : 0.0;
    return maxacc;
}

static double axis_motion_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxvel = get_axis_max_velocity();

    canon_debug(" in axis_motion_time\n");
    // For active axes, find the time required to reach the displacement in each axis
    int ind = 0;
    for (ind = 0; ind < 9; ++ind) {
        double v = maxvel[ind];
        if (v > 0.0) {
            times[ind] = fabs(disp[ind]) / v;
        } else {
            times[ind]=0;
        }
        canon_debug("  ind = %d, maxvel = %f, disp = %f, time = %f\n", ind, v, disp[ind], times[ind]);
    }

    return times.max();
}

// NOTE: not exactly times, comment TODO
static double axis_acc_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxacc = get_axis_max_acceleration();

    for (int i = 0; i < 9; ++i) {
        double a = maxacc[i];
        if (a > 0.0) {
            times[i] = fabs(disp[i]) / a;
        } else {
            times[i]=0;
        }
    }

    return times.max();
}
#endif

void ARC_FEED(int line_number,
              double first_end, double second_end,
	      double first_axis, double second_axis, int rotation,
	      double axis_end_point, 
              double a, double b, double c,
              double u, double v, double w)
{

    EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;

    canon_debug("line = %d\n", line_number);
    canon_debug("first_end = %f, second_end = %f\n", first_end,second_end);

    if( activePlane == CANON_PLANE_XY && canonMotionMode == CANON_CONTINUOUS) {
        double mx, my;
        double lx, ly, lz;
        double unused;

        get_last_pos(lx, ly, lz);

        a = FROM_PROG_ANG(a);
        b = FROM_PROG_ANG(b);
        c = FROM_PROG_ANG(c);
        u = FROM_PROG_LEN(u);
        v = FROM_PROG_LEN(v);
        w = FROM_PROG_LEN(w);

        double fe=FROM_PROG_LEN(first_end), se=FROM_PROG_LEN(second_end), ae=FROM_PROG_LEN(axis_end_point);
        double fa=FROM_PROG_LEN(first_axis), sa=FROM_PROG_LEN(second_axis);
        rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused);
        rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused);
        if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canonNaivecamTolerance) {
            rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w);
            see_segment(line_number, mx, my,
                        (lz + ae)/2, 
                        (canonEndPoint.a + a)/2, 
                        (canonEndPoint.b + b)/2, 
                        (canonEndPoint.c + c)/2, 
                        (canonEndPoint.u + u)/2, 
                        (canonEndPoint.v + v)/2, 
                        (canonEndPoint.w + w)/2);
            see_segment(line_number, fe, se, ae, a, b, c, u, v, w);
            return;
        }
    }

    linearMoveMsg.feed_mode = feed_mode;
    circularMoveMsg.feed_mode = feed_mode;
    flush_segments();

    // Start by defining 3D points for the motion end and center.
    PM_CARTESIAN end_cart(first_end, second_end, axis_end_point);
    PM_CARTESIAN center_cart(first_axis, second_axis, axis_end_point);
    PM_CARTESIAN normal_cart(0.0,0.0,1.0);
    PM_CARTESIAN plane_x(1.0,0.0,0.0);
    PM_CARTESIAN plane_y(0.0,1.0,0.0);
<commit_msg>canon: fix bug #439, non-NCD arcs on machines with ABCUVW axes<commit_after>/********************************************************************
* Description: emccanon.cc
*   Canonical definitions for 3-axis NC application
*
*   Derived from a work by <NAME> & <NAME>
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
********************************************************************/
/*

  Notes:

  Units
  -----
  Values are stored internally as mm and degree units, e.g, program
  offsets, end point, tool length offset.  These are "internal
  units". "External units" are the units used by the EMC motion planner.
  All lengths and units output by the interpreter are converted to
  internal units here, using FROM_PROG_LEN,ANG, and then
  TO_EXT_LEN(),ANG are called to convert these to external units.

  Tool Length Offsets
  -------------------
  The interpreter does not subtract off tool length offsets. It calls
  USE_TOOL_LENGTH_OFFSETS(length), which we record here and apply to
  all appropriate values subsequently.
  */

#include "config.h"
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>		// strncpy()
#include <ctype.h>		// isspace()
#include "emc.hh"		// EMC NML
#include "emc_nml.hh"
#include "canon.hh"
#include "canon_position.hh"		// data type for a machine position
#include "interpl.hh"		// interp_list
#include "emcglb.h"		// TRAJ_MAX_VELOCITY

//#define EMCCANON_DEBUG

//Simple compile-time debug macro
#ifdef EMCCANON_DEBUG
#define canon_debug(...) printf(__VA_ARGS__)
#else
#define canon_debug(...)
#endif

static int debug_velacc = 0;
static double css_maximum, css_numerator; // both always positive
static int spindle_dir = 0;

static const double tiny = 1e-7;
static double xy_rotation = 0.;
static int rotary_unlock_for_traverse = -1;

#ifndef MIN
#define MIN(a,b) ((a)<(b)?(a):(b))
#endif

#ifndef MIN3
#define MIN3(a,b,c) (MIN(MIN((a),(b)),(c)))
#endif

#ifndef MAX
#define MAX(a,b) ((a)>(b)?(a):(b))
#endif

#ifndef MAX3
#define MAX3(a,b,c) (MAX(MAX((a),(b)),(c)))
#endif

#ifndef MAX4
#define MAX4(a,b,c,d) (MAX(MAX((a),(b)),MAX((c),(d))))
#endif

#ifndef MAX9
#define MAX9(a,b,c,d,e,f,g,h,i) (MAX3((MAX3(a,b,c)),(MAX3(d,e,f)),(MAX3(g,h,i))))
#endif

/* macros for converting internal (mm/deg) units to external units */
#define TO_EXT_LEN(mm) ((mm) * GET_EXTERNAL_LENGTH_UNITS())
#define TO_EXT_ANG(deg) ((deg) * GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting external units to internal (mm/deg) units */
#define FROM_EXT_LEN(ext) ((ext) / GET_EXTERNAL_LENGTH_UNITS())
#define FROM_EXT_ANG(ext) ((ext) / GET_EXTERNAL_ANGLE_UNITS())

/* macros for converting internal (mm/deg) units to program units */
#define TO_PROG_LEN(mm) ((mm) / (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define TO_PROG_ANG(deg) (deg)

/* macros for converting program units to internal (mm/deg) units */
#define FROM_PROG_LEN(prog) ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define FROM_PROG_ANG(prog) (prog)

/* Certain axes are periodic.  Hardcode this for now */
#define IS_PERIODIC(axisnum) \
    ((axisnum) == 3 || (axisnum) == 4 || (axisnum) == 5)

// this doesn't quite work yet: disable
#undef IS_PERIODIC
#define IS_PERIODIC(axisnum) (0)

#define AXIS_PERIOD(axisnum) (IS_PERIODIC(axisnum) ? 360 : 0)

//KLUDGE kinematic data struct (instead of returning a single float value)
//FIXME This should really be refactored into a more general structure, but this
//means tearing up the getStraightXXX functions, which probably means
//converting to canon_position operators
struct VelData {
    double tmax;
    double vel;
    double dtot;
};

struct AccelData{
    double tmax;
    double acc;
    double dtot;
};

static PM_QUATERNION quat(1, 0, 0, 0);

static void flush_segments(void);


/*
  These decls were from the old 3-axis canon.hh, and refer functions
  defined here that are used for convenience but no longer have decls
  in the 6-axis canon.hh. So, we declare them here now.
*/
extern void CANON_ERROR(const char *fmt, ...) __attribute__((format(printf,1,2)));

/*
  Origin offsets, length units, and active plane are all maintained
  here in this file. Controller runs in absolute mode, and does not
  have plane select concept.

  programOrigin is stored in mm always, and converted when set or read.
  When it's applied to positions, convert positions to mm units first
  and then add programOrigin.

  Units are then converted from mm to external units, as reported by
  the GET_EXTERNAL_LENGTH_UNITS() function.
  */
static CANON_POSITION g5xOffset(0.0, 0.0, 0.0, 
                                0.0, 0.0, 0.0,
                                0.0, 0.0, 0.0);
static CANON_POSITION g92Offset(0.0, 0.0, 0.0, 
                                0.0, 0.0, 0.0,
                                0.0, 0.0, 0.0);
static CANON_UNITS lengthUnits = CANON_UNITS_MM;
static CANON_PLANE activePlane = CANON_PLANE_XY;

static int feed_mode = 0;
static int synched = 0;

/* Tool length offset is saved here */
static EmcPose currentToolOffset;

#ifndef D2R
#define D2R(r) ((r)*M_PI/180.0)
#endif

static void rotate(double &x, double &y, double theta) {
    double xx, yy;
    double t = D2R(theta);
    xx = x, yy = y;
    x = xx * cos(t) - yy * sin(t); 
    y = xx * sin(t) + yy * cos(t);
}


/**
 * Implementation of planar rotation for a 3D vector.
 * This is basically a shortcut for "rotate" when the values are stored in a
 * cartesian vector.
 * The use of static "xy_rotation" is ugly here, but is at least consistent.
 */
static void to_rotated(PM_CARTESIAN &vec) {
    rotate(vec.x,vec.y,xy_rotation);
}
#if 0
static void from_rotated(PM_CARTESIAN &vec) {
    rotate(vec.x,vec.y,-xy_rotation);
}
#endif
static void rotate_and_offset(CANON_POSITION & pos) {

    pos += g92Offset;

    rotate(pos.x, pos.y, xy_rotation);

    pos += g5xOffset;

    pos += currentToolOffset;
}

static void rotate_and_offset_xyz(PM_CARTESIAN & xyz) {

    xyz += g92Offset.xyz();

    rotate(xyz.x, xyz.y, xy_rotation);

    xyz += g5xOffset.xyz();

    xyz += PM_CARTESIAN(currentToolOffset.tran.x,
            currentToolOffset.tran.y,
            currentToolOffset.tran.z);
}

static CANON_POSITION unoffset_and_unrotate_pos(const CANON_POSITION pos) {
    CANON_POSITION res;

    res = pos;

    res -= currentToolOffset;
    
    res -= g5xOffset;

    rotate(res.x, res.y, -xy_rotation);

    res -= g92Offset;

    return res;
}

static void rotate_and_offset_pos(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {

    x += g92Offset.x;
    y += g92Offset.y;
    z += g92Offset.z;
    a += g92Offset.a;
    b += g92Offset.b;
    c += g92Offset.c;
    u += g92Offset.u;
    v += g92Offset.v;
    w += g92Offset.w;

    rotate(x, y, xy_rotation);

    x += g5xOffset.x;
    y += g5xOffset.y;
    z += g5xOffset.z;
    a += g5xOffset.a;
    b += g5xOffset.b;
    c += g5xOffset.c;
    u += g5xOffset.u;
    v += g5xOffset.v;
    w += g5xOffset.w;

    x += currentToolOffset.tran.x;
    y += currentToolOffset.tran.y;
    z += currentToolOffset.tran.z;
    a += currentToolOffset.a;
    b += currentToolOffset.b;
    c += currentToolOffset.c;
    u += currentToolOffset.u;
    v += currentToolOffset.v;
    w += currentToolOffset.w;
}


static CANON_POSITION unoffset_and_unrotate_pos(const EmcPose pos) {
    CANON_POSITION res(pos);
    return unoffset_and_unrotate_pos(res);
}

// for c in "xyzabcuvw": print "    %s = offset_%s(%s)" % (c,c,c)

static void from_prog(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {
    x = FROM_PROG_LEN(x);
    y = FROM_PROG_LEN(y);
    z = FROM_PROG_LEN(z);
    a = FROM_PROG_ANG(a);
    b = FROM_PROG_ANG(b);
    c = FROM_PROG_ANG(c);
    u = FROM_PROG_LEN(u);
    v = FROM_PROG_LEN(v);
    w = FROM_PROG_LEN(w);
}

static void from_prog(CANON_POSITION &pos) {
    pos.x = FROM_PROG_LEN(pos.x);
    pos.y = FROM_PROG_LEN(pos.y);
    pos.z = FROM_PROG_LEN(pos.z);
    pos.a = FROM_PROG_ANG(pos.a);
    pos.b = FROM_PROG_ANG(pos.b);
    pos.c = FROM_PROG_ANG(pos.c);
    pos.u = FROM_PROG_LEN(pos.u);
    pos.v = FROM_PROG_LEN(pos.v);
    pos.w = FROM_PROG_LEN(pos.w);
}

static void from_prog_len(PM_CARTESIAN &vec) {
    vec.x = FROM_PROG_LEN(vec.x);
    vec.y = FROM_PROG_LEN(vec.y);
    vec.z = FROM_PROG_LEN(vec.z);
}
#if 0
static void to_ext(double &x, double &y, double &z, double &a, double &b, double &c, double &u, double &v, double &w) {
    x = TO_EXT_LEN(x);
    y = TO_EXT_LEN(y);
    z = TO_EXT_LEN(z);
    a = TO_EXT_ANG(a);
    b = TO_EXT_ANG(b);
    c = TO_EXT_ANG(c);
    u = TO_EXT_LEN(u);
    v = TO_EXT_LEN(v);
    w = TO_EXT_LEN(w);
}

static void to_ext(CANON_POSITION & pos) {
    pos.x=TO_EXT_LEN(pos.x);
    pos.y=TO_EXT_LEN(pos.y);
    pos.z=TO_EXT_LEN(pos.z);
    pos.a=TO_EXT_ANG(pos.a);
    pos.b=TO_EXT_ANG(pos.b);
    pos.c=TO_EXT_ANG(pos.c);
    pos.u=TO_EXT_LEN(pos.u);
    pos.v=TO_EXT_LEN(pos.v);
    pos.w=TO_EXT_LEN(pos.w);
}
#endif

static PM_CARTESIAN to_ext_len(const PM_CARTESIAN & pos) {
    PM_CARTESIAN ret;
    ret.x = TO_EXT_LEN(pos.x);
    ret.y = TO_EXT_LEN(pos.y);
    ret.z = TO_EXT_LEN(pos.z);
    return ret;
}

static EmcPose to_ext_pose(double x, double y, double z, double a, double b, double c, double u, double v, double w) {
    EmcPose result;
    result.tran.x = TO_EXT_LEN(x);
    result.tran.y = TO_EXT_LEN(y);
    result.tran.z = TO_EXT_LEN(z);
    result.a = TO_EXT_ANG(a);
    result.b = TO_EXT_ANG(b);
    result.c = TO_EXT_ANG(c);
    result.u = TO_EXT_LEN(u);
    result.v = TO_EXT_LEN(v);
    result.w = TO_EXT_LEN(w);
    return result;
}

static EmcPose to_ext_pose(const CANON_POSITION & pos) {
    EmcPose result;
    result.tran.x = TO_EXT_LEN(pos.x);
    result.tran.y = TO_EXT_LEN(pos.y);
    result.tran.z = TO_EXT_LEN(pos.z);
    result.a = TO_EXT_ANG(pos.a);
    result.b = TO_EXT_ANG(pos.b);
    result.c = TO_EXT_ANG(pos.c);
    result.u = TO_EXT_LEN(pos.u);
    result.v = TO_EXT_LEN(pos.v);
    result.w = TO_EXT_LEN(pos.w);
    return result;
}

static void to_prog(CANON_POSITION &e) {
    e.x = TO_PROG_LEN(e.x);
    e.y = TO_PROG_LEN(e.y);
    e.z = TO_PROG_LEN(e.z);
    e.a = TO_PROG_ANG(e.a);
    e.b = TO_PROG_ANG(e.b);
    e.c = TO_PROG_ANG(e.c);
    e.u = TO_PROG_LEN(e.u);
    e.v = TO_PROG_LEN(e.v);
    e.w = TO_PROG_LEN(e.w);
}

static int axis_valid(int n) {
    return emcStatus->motion.traj.axis_mask & (1<<n);
}

/*
  canonEndPoint is the last programmed end point, stored in case it's
  needed for subsequent calculations. It's in absolute frame, mm units.

  note that when segments are queued for the naive cam detector that the
  canonEndPoint may not be the last programmed endpoint.  get_last_pos()
  retrieves the xyz position after the last of the queued segments.  these
  are also in absolute frame, mm units.
  */
static CANON_POSITION canonEndPoint;
static void canonUpdateEndPoint(double x, double y, double z, 
                                double a, double b, double c,
                                double u, double v, double w)
{
    canonEndPoint.x = x;
    canonEndPoint.y = y;
    canonEndPoint.z = z;

    canonEndPoint.a = a;
    canonEndPoint.b = b;
    canonEndPoint.c = c;

    canonEndPoint.u = u;
    canonEndPoint.v = v;
    canonEndPoint.w = w;
}

static void canonUpdateEndPoint(const CANON_POSITION & pos)
{
    canonEndPoint = pos;
}

/* External call to update the canon end point.
   Called by emctask during skipping of lines (run-from-line) */
void CANON_UPDATE_END_POINT(double x, double y, double z, 
			    double a, double b, double c, 
			    double u, double v, double w)
{
    canonUpdateEndPoint(FROM_PROG_LEN(x),FROM_PROG_LEN(y),FROM_PROG_LEN(z),
    			FROM_PROG_ANG(a),FROM_PROG_ANG(b),FROM_PROG_ANG(c),
			FROM_PROG_LEN(u),FROM_PROG_LEN(v),FROM_PROG_LEN(w));
}


/* motion control mode is used to signify blended v. stop-at-end moves.
   Set to 0 (invalid) at start, so first call will send command out */
static CANON_MOTION_MODE canonMotionMode = 0;

/* motion path-following tolerance is used to set the max path-following
   deviation during CANON_CONTINUOUS.
   If this param is 0, then it will behave as emc always did, allowing
   almost any deviation trying to keep speed up. */
static double canonMotionTolerance = 0.0;

static double canonNaivecamTolerance = 0.0;

/* Spindle speed is saved here */
static double spindleSpeed = 0.0; // always positive

/* Prepped tool is saved here */
static int preppedTool = 0;

/* optional program stop */
static bool optional_program_stop = ON; //set enabled by default (previous EMC behaviour)

/* optional block delete */
static bool block_delete = ON; //set enabled by default (previous EMC behaviour)

/*
  Feed rate is saved here; values are in mm/sec or deg/sec.
  It will be initially set in INIT_CANON() below.
*/
static double currentLinearFeedRate = 0.0;
static double currentAngularFeedRate = 0.0;

/* Used to indicate whether the current move is linear, angular, or 
   a combination of both. */
   //AJ says: linear means axes XYZ move (lines or even circles)
   //         angular means axes ABC move
static int cartesian_move = 0;
static int angular_move = 0;

static double toExtVel(double vel) {
    if (cartesian_move && !angular_move) {
	return TO_EXT_LEN(vel);
    } else if (!cartesian_move && angular_move) {
	return TO_EXT_ANG(vel);
    } else if (cartesian_move && angular_move) {
	return TO_EXT_LEN(vel);
    } else { //seems this case was forgotten, neither linear, neither angular move (we are only sending vel)
	return TO_EXT_LEN(vel);
    }	
}

static double toExtAcc(double acc) { return toExtVel(acc); }

static void send_g5x_msg(int index) {
    flush_segments();

    /* append it to interp list so it gets updated at the right time, not at
       read-ahead time */
    EMC_TRAJ_SET_G5X set_g5x_msg;

    set_g5x_msg.g5x_index = index;

    set_g5x_msg.origin = to_ext_pose(g5xOffset);

    if(css_maximum) {
        SET_SPINDLE_SPEED(spindleSpeed);
    }
    interp_list.append(set_g5x_msg);
}

static void send_g92_msg(void) {
    flush_segments();

    /* append it to interp list so it gets updated at the right time, not at
       read-ahead time */
    EMC_TRAJ_SET_G92 set_g92_msg;

    set_g92_msg.origin = to_ext_pose(g92Offset);

    if(css_maximum) {
        SET_SPINDLE_SPEED(spindleSpeed);
    }
    interp_list.append(set_g92_msg);
}

void SET_XY_ROTATION(double t) {
    EMC_TRAJ_SET_ROTATION sr;
    sr.rotation = t;
    interp_list.append(sr);

    xy_rotation = t;
}

void SET_G5X_OFFSET(int index,
                    double x, double y, double z,
                    double a, double b, double c,
                    double u, double v, double w)
{
    CANON_POSITION pos(x,y,z,a,b,c,u,v,w);
    from_prog(pos);

    /* convert to mm units */
    g5xOffset = pos;

    send_g5x_msg(index);
}

void SET_G92_OFFSET(double x, double y, double z,
                    double a, double b, double c,
                    double u, double v, double w) {
    /* convert to mm units */
    CANON_POSITION pos(x,y,z,a,b,c,u,v,w);
    from_prog(pos);

    g92Offset = pos;

    send_g92_msg();
}

void USE_LENGTH_UNITS(CANON_UNITS in_unit)
{
    lengthUnits = in_unit;

    emcStatus->task.programUnits = in_unit;
}

/* Free Space Motion */
void SET_TRAVERSE_RATE(double rate)
{
    // nothing need be done here
}

void SET_FEED_MODE(int mode) {
    flush_segments();
    feed_mode = mode;
    if(feed_mode == 0) STOP_SPEED_FEED_SYNCH();
}

void SET_FEED_RATE(double rate)
{

    if(feed_mode) {
	START_SPEED_FEED_SYNCH(rate, 1);
	currentLinearFeedRate = rate;
    } else {
	/* convert from /min to /sec */
	rate /= 60.0;


	/* convert to traj units (mm & deg) if needed */
	double newLinearFeedRate = FROM_PROG_LEN(rate),
	       newAngularFeedRate = FROM_PROG_ANG(rate);

	if(newLinearFeedRate != currentLinearFeedRate
		|| newAngularFeedRate != currentAngularFeedRate)
	    flush_segments();

	currentLinearFeedRate = newLinearFeedRate;
	currentAngularFeedRate = newAngularFeedRate;
    }
}

void SET_FEED_REFERENCE(CANON_FEED_REFERENCE reference)
{
    // nothing need be done here
}

/**
 * Get the limiting acceleration for a displacement from the current position to the given position.
 * returns a single acceleration that is the minimum of all axis accelerations.
 */
static AccelData getStraightAcceleration(double x, double y, double z,
                               double a, double b, double c,
                               double u, double v, double w)
{
    double dx, dy, dz, du, dv, dw, da, db, dc;
    double tx, ty, tz, tu, tv, tw, ta, tb, tc;
    AccelData out;

    out.acc = 0.0; // if a move to nowhere
    out.tmax = 0.0;
    out.dtot = 0.0;

    // Compute absolute travel distance for each axis:
    dx = fabs(x - canonEndPoint.x);
    dy = fabs(y - canonEndPoint.y);
    dz = fabs(z - canonEndPoint.z);
    da = fabs(a - canonEndPoint.a);
    db = fabs(b - canonEndPoint.b);
    dc = fabs(c - canonEndPoint.c);
    du = fabs(u - canonEndPoint.u);
    dv = fabs(v - canonEndPoint.v);
    dw = fabs(w - canonEndPoint.w);

    if(!axis_valid(0) || dx < tiny) dx = 0.0;
    if(!axis_valid(1) || dy < tiny) dy = 0.0;
    if(!axis_valid(2) || dz < tiny) dz = 0.0;
    if(!axis_valid(3) || da < tiny) da = 0.0;
    if(!axis_valid(4) || db < tiny) db = 0.0;
    if(!axis_valid(5) || dc < tiny) dc = 0.0;
    if(!axis_valid(6) || du < tiny) du = 0.0;
    if(!axis_valid(7) || dv < tiny) dv = 0.0;
    if(!axis_valid(8) || dw < tiny) dw = 0.0;

    if(debug_velacc) 
        printf("getStraightAcceleration dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g ", 
               dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making.  This is used to determine
    // the units of vel/acc.
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 &&
        du <= 0.0 && dv <= 0.0 && dw <= 0.0) {
	cartesian_move = 0;
    } else {
	cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {
	angular_move = 0;
    } else {
	angular_move = 1;
    }

    // Pure linear move:
    if (cartesian_move && !angular_move) {
	tx = dx? (dx / FROM_EXT_LEN(axis_max_acceleration[0])): 0.0;
	ty = dy? (dy / FROM_EXT_LEN(axis_max_acceleration[1])): 0.0;
	tz = dz? (dz / FROM_EXT_LEN(axis_max_acceleration[2])): 0.0;
	tu = du? (du / FROM_EXT_LEN(axis_max_acceleration[6])): 0.0;
	tv = dv? (dv / FROM_EXT_LEN(axis_max_acceleration[7])): 0.0;
	tw = dw? (dw / FROM_EXT_LEN(axis_max_acceleration[8])): 0.0;
        out.tmax = MAX3(tx, ty ,tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);
        
	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    // Pure angular move:
    else if (!cartesian_move && angular_move) {
	ta = da? (da / FROM_EXT_ANG(axis_max_acceleration[3])): 0.0;
	tb = db? (db / FROM_EXT_ANG(axis_max_acceleration[4])): 0.0;
	tc = dc? (dc / FROM_EXT_ANG(axis_max_acceleration[5])): 0.0;
        out.tmax = MAX3(ta, tb, tc);

	out.dtot = sqrt(da * da + db * db + dc * dc);
	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    // Combination angular and linear move:
    else if (cartesian_move && angular_move) {
	tx = dx? (dx / FROM_EXT_LEN(axis_max_acceleration[0])): 0.0;
	ty = dy? (dy / FROM_EXT_LEN(axis_max_acceleration[1])): 0.0;
	tz = dz? (dz / FROM_EXT_LEN(axis_max_acceleration[2])): 0.0;
	ta = da? (da / FROM_EXT_ANG(axis_max_acceleration[3])): 0.0;
	tb = db? (db / FROM_EXT_ANG(axis_max_acceleration[4])): 0.0;
	tc = dc? (dc / FROM_EXT_ANG(axis_max_acceleration[5])): 0.0;
	tu = du? (du / FROM_EXT_LEN(axis_max_acceleration[6])): 0.0;
	tv = dv? (dv / FROM_EXT_LEN(axis_max_acceleration[7])): 0.0;
	tw = dw? (dw / FROM_EXT_LEN(axis_max_acceleration[8])): 0.0;
        out.tmax = MAX9(tx, ty, tz,
                    ta, tb, tc,
                    tu, tv, tw);

    if(debug_velacc)
        printf("getStraightAcceleration t^2 tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n", 
               tx, ty, tz, ta, tb, tc, tu, tv, tw);
/*  According to NIST IR6556 Section 2.1.2.5 Paragraph A
    a combnation move is handled like a linear move, except
    that the angular axes are allowed sufficient time to
    complete their motion coordinated with the motion of
    the linear axes.
*/
        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

	if (out.tmax > 0.0) {
	    out.acc = out.dtot / out.tmax;
	}
    }
    if(debug_velacc) 
        printf("cartesian %d ang %d acc %g\n", cartesian_move, angular_move, out.acc);
    return out;
}

static AccelData getStraightAcceleration(CANON_POSITION pos)
{

    return getStraightAcceleration(pos.x,
            pos.y,
            pos.z,
            pos.a,
            pos.b,
            pos.c,
            pos.u,
            pos.v,
            pos.w);
}

static VelData getStraightVelocity(double x, double y, double z,
			   double a, double b, double c,
                           double u, double v, double w)
{
    double dx, dy, dz, da, db, dc, du, dv, dw;
    double tx, ty, tz, ta, tb, tc, tu, tv, tw;
    VelData out;

/* If we get a move to nowhere (!cartesian_move && !angular_move)
   we might as well go there at the currentLinearFeedRate...
*/
    out.vel = currentLinearFeedRate;
    out.tmax = 0;
    out.dtot = 0;

    // Compute absolute travel distance for each axis:
    dx = fabs(x - canonEndPoint.x);
    dy = fabs(y - canonEndPoint.y);
    dz = fabs(z - canonEndPoint.z);
    da = fabs(a - canonEndPoint.a);
    db = fabs(b - canonEndPoint.b);
    dc = fabs(c - canonEndPoint.c);
    du = fabs(u - canonEndPoint.u);
    dv = fabs(v - canonEndPoint.v);
    dw = fabs(w - canonEndPoint.w);

    if(!axis_valid(0) || dx < tiny) dx = 0.0;
    if(!axis_valid(1) || dy < tiny) dy = 0.0;
    if(!axis_valid(2) || dz < tiny) dz = 0.0;
    if(!axis_valid(3) || da < tiny) da = 0.0;
    if(!axis_valid(4) || db < tiny) db = 0.0;
    if(!axis_valid(5) || dc < tiny) dc = 0.0;
    if(!axis_valid(6) || du < tiny) du = 0.0;
    if(!axis_valid(7) || dv < tiny) dv = 0.0;
    if(!axis_valid(8) || dw < tiny) dw = 0.0;

    if(debug_velacc) 
        printf("getStraightVelocity dx %g dy %g dz %g da %g db %g dc %g du %g dv %g dw %g\n",
               dx, dy, dz, da, db, dc, du, dv, dw);

    // Figure out what kind of move we're making:
    if (dx <= 0.0 && dy <= 0.0 && dz <= 0.0 &&
        du <= 0.0 && dv <= 0.0 && dw <= 0.0) {
	cartesian_move = 0;
    } else {
	cartesian_move = 1;
    }
    if (da <= 0.0 && db <= 0.0 && dc <= 0.0) {
	angular_move = 0;
    } else {
	angular_move = 1;
    }

    // Pure linear move:
    if (cartesian_move && !angular_move) {
        tx = dx? fabs(dx / FROM_EXT_LEN(axis_max_velocity[0])): 0.0;
        ty = dy? fabs(dy / FROM_EXT_LEN(axis_max_velocity[1])): 0.0;
        tz = dz? fabs(dz / FROM_EXT_LEN(axis_max_velocity[2])): 0.0;
        tu = du? fabs(du / FROM_EXT_LEN(axis_max_velocity[6])): 0.0;
        tv = dv? fabs(dv / FROM_EXT_LEN(axis_max_velocity[7])): 0.0;
        tw = dw? fabs(dw / FROM_EXT_LEN(axis_max_velocity[8])): 0.0;
        out.tmax = MAX3(tx, ty ,tz);
        out.tmax = MAX4(tu, tv, tw, out.tmax);

        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0) {
            out.vel = currentLinearFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Pure angular move:
    else if (!cartesian_move && angular_move) {
        ta = da? fabs(da / FROM_EXT_ANG(axis_max_velocity[3])):0.0;
        tb = db? fabs(db / FROM_EXT_ANG(axis_max_velocity[4])):0.0;
        tc = dc? fabs(dc / FROM_EXT_ANG(axis_max_velocity[5])):0.0;
        out.tmax = MAX3(ta, tb, tc);

        out.dtot = sqrt(da * da + db * db + dc * dc);
        if (out.tmax <= 0.0) {
            out.vel = currentAngularFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    // Combination angular and linear move:
    else if (cartesian_move && angular_move) {
        tx = dx? fabs(dx / FROM_EXT_LEN(axis_max_velocity[0])): 0.0;
        ty = dy? fabs(dy / FROM_EXT_LEN(axis_max_velocity[1])): 0.0;
        tz = dz? fabs(dz / FROM_EXT_LEN(axis_max_velocity[2])): 0.0;
        ta = da? fabs(da / FROM_EXT_ANG(axis_max_velocity[3])): 0.0;
        tb = db? fabs(db / FROM_EXT_ANG(axis_max_velocity[4])): 0.0;
        tc = dc? fabs(dc / FROM_EXT_ANG(axis_max_velocity[5])): 0.0;
        tu = du? fabs(du / FROM_EXT_LEN(axis_max_velocity[6])): 0.0;
        tv = dv? fabs(dv / FROM_EXT_LEN(axis_max_velocity[7])): 0.0;
        tw = dw? fabs(dw / FROM_EXT_LEN(axis_max_velocity[8])): 0.0;
        out.tmax = MAX9(tx, ty, tz,
                ta, tb, tc,
                tu, tv, tw);

        if(debug_velacc)
            printf("getStraightVelocity times tx %g ty %g tz %g ta %g tb %g tc %g tu %g tv %g tw %g\n",
                    tx, ty, tz, ta, tb, tc, tu, tv, tw);

        /*  According to NIST IR6556 Section 2.1.2.5 Paragraph A
            a combnation move is handled like a linear move, except
            that the angular axes are allowed sufficient time to
            complete their motion coordinated with the motion of
            the linear axes.
            */
        if(dx || dy || dz)
            out.dtot = sqrt(dx * dx + dy * dy + dz * dz);
        else
            out.dtot = sqrt(du * du + dv * dv + dw * dw);

        if (out.tmax <= 0.0) {
            out.vel = currentLinearFeedRate;
        } else {
            out.vel = out.dtot / out.tmax;
        }
    }
    if(debug_velacc) 
        printf("cartesian %d ang %d vel %g\n", cartesian_move, angular_move, out.vel);
    return out;
}

static VelData getStraightVelocity(CANON_POSITION pos)
{

    return getStraightVelocity(pos.x,
            pos.y,
            pos.z,
            pos.a,
            pos.b,
            pos.c,
            pos.u,
            pos.v,
            pos.w);
}

#include <vector>
struct pt { double x, y, z, a, b, c, u, v, w; int line_no;};

static std::vector<struct pt> chained_points;

static void flush_segments(void) {
    if(chained_points.empty()) return;

    struct pt &pos = chained_points.back();

    double x = pos.x, y = pos.y, z = pos.z;
    double a = pos.a, b = pos.b, c = pos.c;
    double u = pos.u, v = pos.v, w = pos.w;
    
    int line_no = pos.line_no;

#ifdef SHOW_JOINED_SEGMENTS
    for(unsigned int i=0; i != chained_points.size(); i++) { printf("."); }
    printf("\n");
#endif

    VelData linedata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    double vel = linedata.vel;

    if (cartesian_move && !angular_move) {
        if (vel > currentLinearFeedRate) {
            vel = currentLinearFeedRate;
        }
    } else if (!cartesian_move && angular_move) {
        if (vel > currentAngularFeedRate) {
            vel = currentAngularFeedRate;
        }
    } else if (cartesian_move && angular_move) {
        if (vel > currentLinearFeedRate) {
            vel = currentLinearFeedRate;
        }
    }


    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = feed_mode;

    // now x, y, z, and b are in absolute mm or degree units
    linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
    linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
    linearMoveMsg.end.tran.z = TO_EXT_LEN(z);

    linearMoveMsg.end.u = TO_EXT_LEN(u);
    linearMoveMsg.end.v = TO_EXT_LEN(v);
    linearMoveMsg.end.w = TO_EXT_LEN(w);

    // fill in the orientation
    linearMoveMsg.end.a = TO_EXT_ANG(a);
    linearMoveMsg.end.b = TO_EXT_ANG(b);
    linearMoveMsg.end.c = TO_EXT_ANG(c);

    linearMoveMsg.vel = toExtVel(vel);
    linearMoveMsg.ini_maxvel = toExtVel(linedata.vel);
    AccelData lineaccdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    double acc = lineaccdata.acc;
    linearMoveMsg.acc = toExtAcc(acc);

    linearMoveMsg.type = EMC_MOTION_TYPE_FEED;
    linearMoveMsg.indexrotary = -1;
    if ((vel && acc) || synched) {
        interp_list.set_line_number(line_no);
        interp_list.append(linearMoveMsg);
    }
    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);

    chained_points.clear();
}

static void get_last_pos(double &lx, double &ly, double &lz) {
    if(chained_points.empty()) {
        lx = canonEndPoint.x;
        ly = canonEndPoint.y;
        lz = canonEndPoint.z;
    } else {
        struct pt &pos = chained_points.back();
        lx = pos.x;
        ly = pos.y;
        lz = pos.z;
    }
}

static bool
linkable(double x, double y, double z, 
         double a, double b, double c, 
         double u, double v, double w) {
    struct pt &pos = chained_points.back();
    if(canonMotionMode != CANON_CONTINUOUS || canonNaivecamTolerance == 0)
        return false;
    //FIXME make this length controlled elsewhere?
    if(chained_points.size() > 100) return false;

    //If ABCUVW motion, then the tangent calculation fails?
    // TODO is there a fundamental reason that we can't handle 9D motion here?
    if(a != pos.a) return false;
    if(b != pos.b) return false;
    if(c != pos.c) return false;
    if(u != pos.u) return false;
    if(v != pos.v) return false;
    if(w != pos.w) return false;

    if(x==canonEndPoint.x && y==canonEndPoint.y && z==canonEndPoint.z) return false;
    
    for(std::vector<struct pt>::iterator it = chained_points.begin();
            it != chained_points.end(); it++) {
        PM_CARTESIAN M(x-canonEndPoint.x, y-canonEndPoint.y, z-canonEndPoint.z),
                     B(canonEndPoint.x, canonEndPoint.y, canonEndPoint.z),
                     P(it->x, it->y, it->z);
        double t0 = dot(M, P-B) / dot(M, M);
        if(t0 < 0) t0 = 0;
        if(t0 > 1) t0 = 1;

        double D = mag(P - (B + t0 * M));
        if(D > canonNaivecamTolerance) return false;
    }
    return true;
}

static void
see_segment(int line_number,
	    double x, double y, double z, 
            double a, double b, double c,
            double u, double v, double w) {
    bool changed_abc = (a != canonEndPoint.a)
        || (b != canonEndPoint.b)
        || (c != canonEndPoint.c);

    bool changed_uvw = (u != canonEndPoint.u)
        || (v != canonEndPoint.v)
        || (w != canonEndPoint.w);

    if(!chained_points.empty() && !linkable(x, y, z, a, b, c, u, v, w)) {
        flush_segments();
    }
    pt pos = {x, y, z, a, b, c, u, v, w, line_number};
    chained_points.push_back(pos);
    if(changed_abc || changed_uvw) {
        flush_segments();
    }
}

void FINISH() {
    flush_segments();
}

void STRAIGHT_TRAVERSE(int line_number,
                       double x, double y, double z,
		       double a, double b, double c,
                       double u, double v, double w)
{
    double vel, acc;

    flush_segments();

    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;

    linearMoveMsg.feed_mode = 0;
    if (rotary_unlock_for_traverse != -1)
        linearMoveMsg.type = EMC_MOTION_TYPE_INDEXROTARY;
    else
        linearMoveMsg.type = EMC_MOTION_TYPE_TRAVERSE;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);

    VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);

    vel = veldata.vel;
    acc = accdata.acc;

    linearMoveMsg.end = to_ext_pose(x,y,z,a,b,c,u,v,w);
    linearMoveMsg.vel = linearMoveMsg.ini_maxvel = toExtVel(vel);
    linearMoveMsg.acc = toExtAcc(acc);
    linearMoveMsg.indexrotary = rotary_unlock_for_traverse;

    int old_feed_mode = feed_mode;
    if(feed_mode)
	STOP_SPEED_FEED_SYNCH();

    if(vel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(linearMoveMsg);
    }

    if(old_feed_mode)
	START_SPEED_FEED_SYNCH(currentLinearFeedRate, 1);

    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

void STRAIGHT_FEED(int line_number,
                   double x, double y, double z, 
                   double a, double b, double c,
                   double u, double v, double w)
{
    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
    linearMoveMsg.feed_mode = feed_mode;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);
    see_segment(line_number, x, y, z, a, b, c, u, v, w);
}


void RIGID_TAP(int line_number, double x, double y, double z)
{
    double ini_maxvel,acc;
    EMC_TRAJ_RIGID_TAP rigidTapMsg;
    double unused=0;

    from_prog(x,y,z,unused,unused,unused,unused,unused,unused);
    rotate_and_offset_pos(x,y,z,unused,unused,unused,unused,unused,unused);

    VelData veldata = getStraightVelocity(x, y, z,
                              canonEndPoint.a, canonEndPoint.b, canonEndPoint.c, 
                              canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);

    ini_maxvel = veldata.vel;
    
    AccelData accdata = getStraightAcceleration(x, y, z,
                                  canonEndPoint.a, canonEndPoint.b, canonEndPoint.c,
                                  canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);
    acc = accdata.acc;
    
    rigidTapMsg.pos = to_ext_pose(x,y,z,
                                 canonEndPoint.a, canonEndPoint.b, canonEndPoint.c,
                                 canonEndPoint.u, canonEndPoint.v, canonEndPoint.w);

    rigidTapMsg.vel = toExtVel(ini_maxvel);
    rigidTapMsg.ini_maxvel = toExtVel(ini_maxvel);
    rigidTapMsg.acc = toExtAcc(acc);

    flush_segments();

    if(ini_maxvel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(rigidTapMsg);
    }

    // don't move the endpoint because after this move, we are back where we started
}


/*
  STRAIGHT_PROBE is exactly the same as STRAIGHT_FEED, except that it
  uses a probe message instead of a linear move message.
*/

void STRAIGHT_PROBE(int line_number,
                    double x, double y, double z, 
                    double a, double b, double c,
                    double u, double v, double w,
                    unsigned char probe_type)
{
    double ini_maxvel, vel, acc;
    EMC_TRAJ_PROBE probeMsg;

    from_prog(x,y,z,a,b,c,u,v,w);
    rotate_and_offset_pos(x,y,z,a,b,c,u,v,w);

    flush_segments();

    VelData veldata = getStraightVelocity(x, y, z, a, b, c, u, v, w);
    ini_maxvel = vel = veldata.vel;

    if (cartesian_move && !angular_move) {
	if (vel > currentLinearFeedRate) {
	    vel = currentLinearFeedRate;
	}
    } else if (!cartesian_move && angular_move) {
	if (vel > currentAngularFeedRate) {
	    vel = currentAngularFeedRate;
	}
    } else if (cartesian_move && angular_move) {
	if (vel > currentLinearFeedRate) {
	    vel = currentLinearFeedRate;
	}
    }

    AccelData accdata = getStraightAcceleration(x, y, z, a, b, c, u, v, w);
    acc = accdata.acc;

    probeMsg.vel = toExtVel(vel);
    probeMsg.ini_maxvel = toExtVel(ini_maxvel);
    probeMsg.acc = toExtAcc(acc);

    probeMsg.type = EMC_MOTION_TYPE_PROBING;
    probeMsg.probe_type = probe_type;

    probeMsg.pos = to_ext_pose(x,y,z,a,b,c,u,v,w);

    if(vel && acc)  {
        interp_list.set_line_number(line_number);
        interp_list.append(probeMsg);
    }
    canonUpdateEndPoint(x, y, z, a, b, c, u, v, w);
}

/* Machining Attributes */

void SET_MOTION_CONTROL_MODE(CANON_MOTION_MODE mode, double tolerance)
{
    EMC_TRAJ_SET_TERM_COND setTermCondMsg;

    flush_segments();

    canonMotionMode = mode;
    canon_debug("canonMotionMode = %d\n", mode);
    canonMotionTolerance =  FROM_PROG_LEN(tolerance);

    switch (mode) {
    case CANON_CONTINUOUS:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND;
        setTermCondMsg.tolerance = TO_EXT_LEN(canonMotionTolerance);
        break;
    case CANON_EXACT_PATH:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_EXACT;
        break;

    case CANON_EXACT_STOP:
    default:
        setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP;
        break;
    }

    interp_list.append(setTermCondMsg);
}

void SET_NAIVECAM_TOLERANCE(double tolerance)
{
    canonNaivecamTolerance =  FROM_PROG_LEN(tolerance);
}

void SELECT_PLANE(CANON_PLANE in_plane)
{
    activePlane = in_plane;
}

void SET_CUTTER_RADIUS_COMPENSATION(double radius)
{
    // nothing need be done here
}

void START_CUTTER_RADIUS_COMPENSATION(int side)
{
    // nothing need be done here
}

void STOP_CUTTER_RADIUS_COMPENSATION()
{
    // nothing need be done here
}



void START_SPEED_FEED_SYNCH(double feed_per_revolution, bool velocity_mode)
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = TO_EXT_LEN(FROM_PROG_LEN(feed_per_revolution));
    spindlesyncMsg.velocity_mode = velocity_mode;
    interp_list.append(spindlesyncMsg);
    synched = 1;
}

void STOP_SPEED_FEED_SYNCH()
{
    flush_segments();
    EMC_TRAJ_SET_SPINDLESYNC spindlesyncMsg;
    spindlesyncMsg.feed_per_revolution = 0.0;
    spindlesyncMsg.velocity_mode = false;
    interp_list.append(spindlesyncMsg);
    synched = 0;
}

/* Machining Functions */
static double chord_deviation(double sx, double sy, double ex, double ey, double cx, double cy, int rotation, double &mx, double &my) {
    double th1 = atan2(sy-cy, sx-cx),
           th2 = atan2(ey-cy, ex-cx),
           r = hypot(sy-cy, sx-cx),
           dth = th2 - th1;

    if(rotation < 0) {
        if(dth >= -1e-5) th2 -= 2*M_PI;
        // in the edge case where atan2 gives you -pi and pi, a second iteration is needed
        // to get these in the right order
        dth = th2 - th1;
        if(dth >= -1e-5) th2 -= 2*M_PI;
    } else {
        if(dth <= 1e-5) th2 += 2*M_PI;
        dth = th2 - th1;
        if(dth <= 1e-5) th2 += 2*M_PI;
    }

    double included = fabs(th2 - th1);
    double mid = (th2 + th1) / 2;
    mx = cx + r * cos(mid);
    my = cy + r * sin(mid);
    double dev = r * (1 - cos(included/2));
    return dev;
}

/* Spline and NURBS additional functions; */

static double max(double a, double b) {
    if(a < b) return b;
    return a;
}
static void unit(double *x, double *y) {
    double h = hypot(*x, *y);
    if(h != 0) { *x/=h; *y/=h; }
}

static void
arc(int lineno, double x0, double y0, double x1, double y1, double dx, double dy) {
    double small = 0.000001;
    double x = x1-x0, y=y1-y0;
    double den = 2 * (y*dx - x*dy);
    CANON_POSITION p = unoffset_and_unrotate_pos(canonEndPoint);
    to_prog(p);
    if (fabs(den) > small) {
        double r = -(x*x+y*y)/den;
        double i = dy*r, j = -dx*r;
        double cx = x0+i, cy=y0+j;
        ARC_FEED(lineno, x1, y1, cx, cy, r<0 ? 1 : -1,
                 p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    } else { 
        STRAIGHT_FEED(lineno, x1, y1, p.z, p.a, p.b, p.c, p.u, p.v, p.w);
    }
}

static int
biarc(int lineno, double p0x, double p0y, double tsx, double tsy,
      double p4x, double p4y, double tex, double tey, double r=1.0) {
    unit(&tsx, &tsy);
    unit(&tex, &tey);

    double vx = p0x - p4x, vy = p0y - p4y;
    double c = vx*vx + vy*vy;
    double b = 2 * (vx * (r*tsx + tex) + vy * (r*tsy + tey));
    double a = 2 * r * (tsx * tex + tsy * tey - 1);

    double discr = b*b - 4*a*c;
    if(discr < 0) return 0;

    double disq = sqrt(discr);
    double beta1 = (-b-disq) / 2 / a;
    double beta2 = (-b+disq) / 2 / a;

    if(beta1 > 0 && beta2 > 0)
      return 0;
    double beta = max(beta1, beta2);
    double alpha = beta * r;
    double ab = alpha + beta;
    double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy,
         p3x = p4x - beta * tex, p3y = p4y - beta * tey,
         p2x = (p1x*beta + p3x*alpha) / ab,
         p2y = (p1y*beta + p3y*alpha) / ab;
    double tmx = p3x-p2x, tmy = p3y-p2y;
    unit(&tmx, &tmy);

    arc(lineno, p0x, p0y, p2x, p2y, tsx, tsy);
    arc(lineno, p2x, p2y, p4x, p4y, tmx, tmy);
    return 1;
}


/* Canon calls */

void NURBS_FEED(int lineno, std::vector<CONTROL_POINT> nurbs_control_points, unsigned int k) {
    flush_segments();

    unsigned int n = nurbs_control_points.size() - 1;
    double umax = n - k + 2;
    unsigned int div = nurbs_control_points.size()*4;
    std::vector<unsigned int> knot_vector = knot_vector_creator(n, k);	
    PLANE_POINT P0, P0T, P1, P1T;

    P0 = nurbs_point(0,k,nurbs_control_points,knot_vector);
    P0T = nurbs_tangent(0, k, nurbs_control_points, knot_vector);

    for(unsigned int i=1; i<=div; i++) {
	double u = umax * i / div;
        P1 = nurbs_point(u,k,nurbs_control_points,knot_vector);
	P1T = nurbs_tangent(u,k,nurbs_control_points,knot_vector);
        biarc(lineno, P0.X,P0.Y, P0T.X,P0T.Y, P1.X,P1.Y, P1T.X,P1T.Y);
        P0 = P1;
        P0T = P1T;
    }
    knot_vector.clear();
}


/**
 * Simple circular shift function for PM_CARTESIAN type.
 * Cycle around axes without changing the individual values. A circshift of -1
 * makes the X value become the new Y, Y become the Z, and Z become the new X.
 */
static PM_CARTESIAN circshift(PM_CARTESIAN & vec, int steps)
{
    int X=0,Y=1,Z=2;

    int s = 3;
    // Use mod to cycle indices around by steps
    X = (X + steps + s) % s;
    Y = (Y + steps + s) % s;
    Z = (Z + steps + s) % s;
    return PM_CARTESIAN(vec[X],vec[Y],vec[Z]);
}

#if 0
static CANON_POSITION get_axis_max_velocity()
{
    CANON_POSITION maxvel;
    maxvel.x = axis_valid(0) ? FROM_EXT_LEN(axis_max_velocity[0]) : 0.0;
    maxvel.y = axis_valid(1) ? FROM_EXT_LEN(axis_max_velocity[1]) : 0.0;
    maxvel.z = axis_valid(2) ? FROM_EXT_LEN(axis_max_velocity[2]) : 0.0;

    maxvel.a = axis_valid(3) ? FROM_EXT_ANG(axis_max_velocity[3]) : 0.0;
    maxvel.b = axis_valid(4) ? FROM_EXT_ANG(axis_max_velocity[4]) : 0.0;
    maxvel.c = axis_valid(5) ? FROM_EXT_ANG(axis_max_velocity[5]) : 0.0;

    maxvel.u = axis_valid(6) ? FROM_EXT_LEN(axis_max_velocity[6]) : 0.0;
    maxvel.v = axis_valid(7) ? FROM_EXT_LEN(axis_max_velocity[7]) : 0.0;
    maxvel.w = axis_valid(8) ? FROM_EXT_LEN(axis_max_velocity[8]) : 0.0;
    return maxvel;
}

static CANON_POSITION get_axis_max_acceleration()
{
    CANON_POSITION maxacc;
    maxacc.x = axis_valid(0) ? FROM_EXT_LEN(axis_max_acceleration[0]) : 0.0;
    maxacc.y = axis_valid(1) ? FROM_EXT_LEN(axis_max_acceleration[1]) : 0.0;
    maxacc.z = axis_valid(2) ? FROM_EXT_LEN(axis_max_acceleration[2]) : 0.0;

    maxacc.a = axis_valid(3) ? FROM_EXT_ANG(axis_max_acceleration[3]) : 0.0;
    maxacc.b = axis_valid(4) ? FROM_EXT_ANG(axis_max_acceleration[4]) : 0.0;
    maxacc.c = axis_valid(5) ? FROM_EXT_ANG(axis_max_acceleration[5]) : 0.0;

    maxacc.u = axis_valid(6) ? FROM_EXT_LEN(axis_max_acceleration[6]) : 0.0;
    maxacc.v = axis_valid(7) ? FROM_EXT_LEN(axis_max_acceleration[7]) : 0.0;
    maxacc.w = axis_valid(8) ? FROM_EXT_LEN(axis_max_acceleration[8]) : 0.0;
    return maxacc;
}

static double axis_motion_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxvel = get_axis_max_velocity();

    canon_debug(" in axis_motion_time\n");
    // For active axes, find the time required to reach the displacement in each axis
    int ind = 0;
    for (ind = 0; ind < 9; ++ind) {
        double v = maxvel[ind];
        if (v > 0.0) {
            times[ind] = fabs(disp[ind]) / v;
        } else {
            times[ind]=0;
        }
        canon_debug("  ind = %d, maxvel = %f, disp = %f, time = %f\n", ind, v, disp[ind], times[ind]);
    }

    return times.max();
}

// NOTE: not exactly times, comment TODO
static double axis_acc_time(const CANON_POSITION & start, const CANON_POSITION & end)
{

    CANON_POSITION disp = end - start;
    CANON_POSITION times; 
    CANON_POSITION maxacc = get_axis_max_acceleration();

    for (int i = 0; i < 9; ++i) {
        double a = maxacc[i];
        if (a > 0.0) {
            times[i] = fabs(disp[i]) / a;
        } else {
            times[i]=0;
        }
    }

    return times.max();
}
#endif

void ARC_FEED(int line_number,
              double first_end, double second_end,
	      double first_axis, double second_axis, int rotation,
	      double axis_end_point, 
              double a, double b, double c,
              double u, double v, double w)
{

    EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
    EMC_TRAJ_LINEAR_MOVE linearMoveMsg;

    canon_debug("line = %d\n", line_number);
    canon_debug("first_end = %f, second_end = %f\n", first_end,second_end);

    if( activePlane == CANON_PLANE_XY && canonMotionMode == CANON_CONTINUOUS) {
        double mx, my;
        double lx, ly, lz;
        double unused;

        get_last_pos(lx, ly, lz);

        double fe=FROM_PROG_LEN(first_end), se=FROM_PROG_LEN(second_end), ae=FROM_PROG_LEN(axis_end_point);
        double fa=FROM_PROG_LEN(first_axis), sa=FROM_PROG_LEN(second_axis);
        rotate_and_offset_pos(fe, se, ae, unused, unused, unused, unused, unused, unused);
        rotate_and_offset_pos(fa, sa, unused, unused, unused, unused, unused, unused, unused);
        if (chord_deviation(lx, ly, fe, se, fa, sa, rotation, mx, my) < canonNaivecamTolerance) {
            a = FROM_PROG_ANG(a);
            b = FROM_PROG_ANG(b);
            c = FROM_PROG_ANG(c);
            u = FROM_PROG_LEN(u);
            v = FROM_PROG_LEN(v);
            w = FROM_PROG_LEN(w);

            rotate_and_offset_pos(unused, unused, unused, a, b, c, u, v, w);
            see_segment(line_number, mx, my,
                        (lz + ae)/2, 
                        (canonEndPoint.a + a)/2, 
                        (canonEndPoint.b + b)/2, 
                        (canonEndPoint.c + c)/2, 
                        (canonEndPoint.u + u)/2, 
                        (canonEndPoint.v + v)/2, 
                        (canonEndPoint.w + w)/2);
            see_segment(line_number, fe, se, ae, a, b, c, u, v, w);
            return;
        }
    }

    linearMoveMsg.feed_mode = feed_mode;
    circularMoveMsg.feed_mode = feed_mode;
    flush_segments();

    // Start by defining 3D points for the motion end and center.
    PM_CARTESIAN end_cart(first_end, second_end, axis_end_point);
    PM_CARTESIAN center_cart(first_axis, second_axis, axis_end_point);
    PM_CARTESIAN normal_cart(0.0,0.0,1.0);
    PM_CARTESIAN plane_x(1.0,0.0,0.0);
    PM_CARTESIAN plane_y(0.0,1.0,0.0);

{'fraction_non_alphanumeric': 0.13319259394892452, 'fraction_numerical': 0.015671617677153905, 'mean_word_length': 2.679613189600507, 'pattern_counts': {'":': 2, '<': 133, '<?xml version=': 0, '>': 77, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 4, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 54, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1648525', 'n_tokens_mistral': 36834, 'n_tokens_neox': 35257, 'n_words': 8698}