/*
 * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * Real-time control interface for CNC machinery and robots.
 */

#include <sys/param.h>
#include <sys/queue.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <sys/vnode.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/device.h>
#include <sys/pool.h>
#include <sys/conf.h>
#include <sys/lock.h>

#include <sys/gpio.h>
#include <sys/cnc.h>

#include <dev/gpio/gpiovar.h>

#include <machine/bus.h>

#include "cnc_devicevar.h"
#include "cnc_servovar.h"
#include "cnc_spindlevar.h"
#include "cnc_estopvar.h"
#include "cnc_encodervar.h"
#include "cnc_mpgvar.h"
#include "cnc_statledvar.h"
#include "cnc_lcdvar.h"

#include "cncvar.h"

#include "servo.h"
#include "spindle.h"
#include "estop.h"
#include "encoder.h"
#include "mpg.h"
#include "cnclcd.h"
#include "cncstatled.h"

#define STEPMAX	(INT64_MAX-1)
#define STEPLEN 20336			/* 20336 units = 1 step */
#define MAXSTEPS (STEPMAX/STEPLEN)

struct cnc_kinlimits cnc_kinlimits = {
	20000,				/* Max steps/sec */
	200000,				/* Max steps/ms^2 */
	8000000				/* Max steps/ms^3 */
};

const char *cnc_axis_names[] = {
	"X", "Y", "Z",			/* Primary axes */
	"U", "V", "W",			/* Secondary axes */
	"I", "J", "K",			/* Arc center vectors */
	"A", "B", "C"			/* Rotary axes */
};

const char *cnc_insn_names[] = {
	"MOVE",
	"JOG",
	"SET_INTERP",
	"SPINDLE_DIR",
	"SPINDLE_SPEED",
	"SPINDLE_START",
	"SPINDLE_STOP",
	"ATC_PREPARE",
	"ATC_CHANGE",
	"LASER_ON",
	"LASER_OFF",
	"LASER_CURRENT",
	"PICKPLACE_REEL",
	"PICKPLACE_SUCTION",
	"PICKPLACE_RELEASE",
	"PREEMPT",
	"DWELL",
	"COOL_MIST",
	"COOL_FLOOD",
	"COOL_VORTEX"
};

struct pool cnc_insnpl;
int cnc_opened;
TAILQ_HEAD(,cnc_insn) cnc_prog;
struct cnc_vector cnc_pos;
struct cnc_timings cnc_timings;

struct servo_softc      *cnc_servos[CNC_MAX_SERVOS];
struct spindle_softc    *cnc_spindles[CNC_MAX_SPINDLES];
struct estop_softc      *cnc_estops[CNC_MAX_ESTOPS];
struct encoder_softc    *cnc_encoders[CNC_MAX_ENCODERS];
struct mpg_softc        *cnc_mpgs[CNC_MAX_MPGS];
struct cnclcd_softc     *cnc_lcds[CNC_MAX_LCDS];
struct cncstatled_softc *cnc_status_led = NULL;

int cnc_nservos = 0;
int cnc_nspindles = 0;
int cnc_nestops = 0;
int cnc_nencoders = 0;
int cnc_nmpgs = 0;
int cnc_nlcds = 0;

void
cncattach(int num)
{
	int i;

	if (num > 1)
		return;

	cnc_opened = 0;
	pool_init(&cnc_insnpl, sizeof(struct cnc_insn), 0, 0, 0, "cncinsnpl",
	    NULL);
	pool_setlowat(&cnc_insnpl, 1024);
	TAILQ_INIT(&cnc_prog);

	for (i = 0; i < CNC_NAXES; i++)
		cnc_pos.v[i] = 0;
	
	cnc_timings.hz = 0;
	cnc_timings.move_jog = 0;
}

int
cncdetach(struct device *self, int flags)
{
#if NCNCLCD > 0
	int i;
	for (i = 0; i < cnc_nlcds; i++)
		cnclcd_close(cnc_lcds[i]);
#endif
	cnc_prog_reset();
	return (0);
}

int
cncactivate(struct device *self, enum devact a)
{
	return (0);
}

int
cncopen(dev_t dev, int flag, int mode, struct proc *p)
{
	if (cnc_opened) {
		return (EBUSY);
	}
	cnc_opened = 1;
	return (0);
}

int
cncclose(dev_t dev, int flag, int mode, struct proc *p)
{
	cnc_prog_reset();
	cnc_opened = 0;
	return (0);
}

int
cncwrite(dev_t dev, struct uio *uio, int ioflag)
{
	const char *cause;
	struct cnc_insn insn, *iNew;
	u_int ip = 0;

	while (uio->uio_resid > 0) {
		int rv;

		rv = uiomove(&insn, sizeof(struct cnc_insn), uio);
		if (rv != 0) {
			printf("cncwrite: uiomove: %d\n", rv);
			return (EIO);
		}
		if (insn.i_type < 0 || insn.i_type >= CNC_LAST_INSN) {
			printf("cncwrite: bad insn %d\n", insn.i_type);
			return (ENODEV);
		}
		/*
		 * We do the best we can to validate the program
		 * before it is executed.
		 */
		switch (insn.i_type) {
#if NSERVO > 0
		case CNC_MOVE:
			if (cnc_vec_distance(&cnc_pos, &insn.i_pos) == 0) {
				cause = "L=0";
				goto fail;
			}
			if (insn.i_Amax == 0) { cause = "Amax=0"; goto fail; }
			if (insn.i_Jmax == 0) { cause = "Jmax=0"; goto fail; }
			break;
		case CNC_JOG:
			break;
#else
		case CNC_MOVE:
		case CNC_JOG:
			cause = "no servo devices available";
			goto fail;
#endif /* NSERVO */
		case CNC_SET_INTERP:
			if (insn.i_interp_mode < 0 ||
			    insn.i_interp_mode >= CNC_INTERP_LAST) {
				cause = "bad interpolation mode";
				goto fail;
			}
			break;
#if NSPINDLE > 0
		case CNC_SPINDLE_DIR:
			if (spindle_find(insn.i_spindle_id) == NULL) {
				cause = "no such spindle";
				goto fail;
			}
			if (insn.i_spindle_dir != -1 &&
			    insn.i_spindle_dir != 1) {
				cause = "bad direction";
				goto fail;
			}
			break;
		case CNC_SPINDLE_SPEED:
			{
				struct spindle_soft *sp;

				if ((sp = spindle_find(insn.i_spindle_id)) != NULL) {
					if (insn.i_spindle_speed > sp->sc_speed_max) {
						cause = "requested speed exceeds "
						        "spindle capabilities";
						goto fail;
					}
				} else {
					cause = "no such spindle";
					goto fail;
				}
			}
			break;
		case CNC_SPINDLE_START:
		case CNC_SPINDLE_STOP:
			if (spindle_find(insn.i_spindle_id) == NULL) {
				cause = "no such spindle";
				goto fail;
			}
			break;
#else /* NSPINDLE */
		case CNC_SPINDLE_DIR:
		case CNC_SPINDLE_SPEED:
		case CNC_SPINDLE_START:
		case CNC_SPINDLE_STOP:
			cause = "no spindle devices available";
			goto fail;
#endif /* NSPINDLE */

		default:
			break;
		}
		if ((iNew = pool_get(&cnc_insnpl, 0)) == NULL) {
			cause = "out of memory for instruction";
			goto fail;
		}
		memcpy(iNew, &insn, sizeof(struct cnc_insn));
		TAILQ_INSERT_TAIL(&cnc_prog, iNew, prog);
		ip++;
	}
	printf("cnc: added %u insns to program queue\n", ip);
	return (0);
fail:
	printf("%s[%d]: %s\n", cnc_insn_names[insn.i_type], ip, cause);
	return (EINVAL);
}

void
cnc_prog_reset(void)
{
	struct cnc_insn *in;
	u_int ninsns = 0;

	while ((in = TAILQ_FIRST(&cnc_prog)) != NULL) {
		TAILQ_REMOVE(&cnc_prog, in, prog);
		pool_put(&cnc_insnpl, in);
		ninsns++;
	}
/*	printf("cnc: deleted %u insns\n", ninsns); */
#if NCNCSTATLED > 0
	cncstatled_set(0);
#endif
}

void
cnc_message(const char *msg)
{
#if NCNCLCD > 0
	int i;
	for (i = 0; i < cnc_nlcds; i++) {
		struct cnclcd_softc *lcd = cnc_lcds[i];
		if (lcd->sc_open) {
			cnclcd_puts(lcd, msg);
			cnclcd_putc(lcd, '\n');
		}
	}
#endif
}

int
cnc_prog_exec(void)
{
	struct cnc_insn *insn;
	u_int ninsns = 0;
	int s;

	s = splhigh();

#if NCNCSTATLED > 0
	/* Initialize error LEDs and establish communication with LCDs. */
	cncstatled_set(0);
#endif

#if NCNCLCD > 0
	{
	int i;

	for (i = 0; i < cnc_nlcds; i++) {
		struct cnclcd_softc *lcd = cnc_lcds[i];

		if (!lcd->sc_open &&
		    cnclcd_open(cnc_lcds[i], 9600, 7, CNCLCD_PEVEN, 1) == -1) {
			printf("%s: cannot open\n", ((struct device *)lcd)->dv_xname);
			continue;
		}
		cnclcd_puts(lcd, ((struct device *)lcd)->dv_xname);
		cnclcd_putc(lcd, '\n');
	}
	}
#endif

	if (cnc_timings.hz == 0 || cnc_timings.move_jog == 0) {
		printf("cnc: timings are not calibrated!\n");
		goto fail;
	}
	if (cnc_nservos != CNC_NAXES) {
		/* XXX */
		printf("cnc: nservos=%d, but CNC_NAXES=%d\n", cnc_nservos,
		    CNC_NAXES);
		goto fail;
	}
	cnc_message("Executing program\n");
	TAILQ_FOREACH(insn, &cnc_prog, prog) {
		if (insn->i_type >= CNC_LAST_INSN) {
			continue;
		}
		cnc_message(cnc_insn_names[insn->i_type]);
		cnc_message("\n");
		switch (insn->i_type) {
#if NSERVO > 0
		case CNC_MOVE:
			if (cnc_move(insn) == -1) {
				goto fail;
			}
			break;
# if NMPG > 0
		case CNC_JOG:
			if (cnc_nmpgs < 1) {
				printf("cnc: JOG: no MPGs are available\n");
				goto fail;
			}
			if (insn->i_mult == 1) {
				if (cnc_move_jog_singlestep(insn) == -1)
					goto fail;
			} else {
				if (cnc_move_jog(insn, 0) == -1)
					goto fail;
			}
			break;
# endif /* NMPG */
#endif /* NSERVO */

#if NSPINDLE > 0
		case CNC_SPINDLE_START:
			if (spindle_start(cnc_spindles[insn->i_spindle_id],
			    insn) == -1) {
				goto fail;
			}
			break;
		case CNC_SPINDLE_STOP:
			if (spindle_stop(cnc_spindles[insn->i_spindle_id],
			    insn) == -1) {
				goto fail;
			}
			break;
		case CNC_SPINDLE_SPEED:
			if (spindle_speed(cnc_spindles[insn->i_spindle_id],
			    insn) == -1) {
				goto fail;
			}
			break;
#endif /* NSPINDLE */

		default:
			printf("cnc: illegal instruction 0x%x\n", insn->i_type);
			break;
		}
		ninsns++;
	}
#if 0
	{
		int i;

		for (i = 0; i < cnc_nlcds; i++) {
			struct cnclcd_softc *lcd = cnc_lcds[i];
			if (lcd->sc_open)
				cnclcd_close(lcd);
		}
	}
#endif
	splx(s);
	return (0);
fail:
#if NCNCSTATLED > 0
	cncstatled_set(1);
#endif
	splx(s);
	return (EIO);
}

/* Calibrate the delay loop for 1Hz reference. */
cnc_utime_t
cnc_calibrate_hz(void)
{
	const int maxIters = 100;
	const cnc_utime_t tTgt = 1e6; /* 1s */
	struct timeval tv1, tv2;
	cnc_utime_t t, r, i;
	cnc_time_t d;
	int s, j;

	/* Start from an arbitrary value. XXX TODO use cpu info */
	r = 380000000U;

	s = splhigh();
	printf("cnc: calibrating delay loop for 1Hz (%d iters max)...",
	    maxIters);
	for (j = 0; j < maxIters; j++) {
		microtime(&tv1);
		for (i = 0; i < r; i++)
			;;
		microtime(&tv2);

		t = (tv2.tv_sec - tv1.tv_sec)*1e6 +
		    (tv2.tv_usec - tv1.tv_usec);

		printf(" %ld", tTgt-t);
		if (t < tTgt) {
			d = tTgt - t;
			if (d < 10) { break; }
			r += d*4;
		} else if (t > tTgt) {
			d = t - tTgt;
			if (d < 10) { break; }
			r -= d*4;
		} else {
			break;
		}
	}
	splx(s);
	printf("...using %llu\n", r);
	return (r);
}

/* Benchmark cnc_move_jog(). */
cnc_utime_t
cnc_calibrate_move_jog(void)
{
#if NMPG > 0
	struct timespec tv1, tv2;
	struct cnc_insn insn;
	cnc_utime_t r;
	int i, s;

	if (cnc_nmpgs < 1)
		return (1);
	
	printf("cnc: simulating cnc_move_jog()...");

	insn.i_type = CNC_JOG;
	insn.i_v0 = 100;
	insn.i_F = 10000;
	insn.i_Amax = 1234;
	insn.i_Jmax = 1234;
	insn.i_mult = 100;
	for (i = 0; i < CNC_NAXES; i++)
		cnc_pos.v[i] = 0.0;

	/* Benchmark the routine using the system clock. */
	s = splhigh();
	nanotime(&tv1);
	cnc_move_jog(&insn, 1);
	nanotime(&tv2);
	splx(s);

	r = (cnc_utime_t)(tv2.tv_sec - tv1.tv_sec)*1e12 +
	                 (tv2.tv_nsec - tv1.tv_nsec);
	printf("...%lluns\n", r);
	return (r);
#else
	return (1);
#endif
}

int
cncioctl(dev_t dev, u_long cmd, caddr_t data, int flag, struct proc *p)
{
	cnc_vec_t *pos;
	int i;

	switch (cmd) {
	case CNC_EXECPROG:
		return (cnc_prog_exec());
	case CNC_RESETPROG:
		return (cnc_prog_exec());
	case CNC_GETPOS:
		pos = (cnc_vec_t *)data;
		for (i = 0; i < CNC_NAXES; i++) {
			pos->v[i] = cnc_pos.v[i];
		}
		return (0);
	case CNC_SETPOS:
		pos = (cnc_vec_t *)data;
		for (i = 0; i < CNC_NAXES; i++) {
			printf("cnc: SETPOS axis#%d: %lu -> %lu\n", i,
			    cnc_pos.v[i], pos->v[i]);
			cnc_pos.v[i] = pos->v[i];
		}
		return (0);
	case CNC_GETNSERVOS:
		*(int *)data = cnc_nservos;
		return (0);
	case CNC_GETNSPINDLES:
		*(int *)data = cnc_nspindles;
		return (0);
	case CNC_GETNESTOPS:
		*(int *)data = cnc_nestops;
		return (0);
	case CNC_GETNENCODERS:
		*(int *)data = cnc_nencoders;
		return (0);
	case CNC_GETNMPGS:
		*(int *)data = cnc_nmpgs;
		return (0);
	case CNC_GETKINLIMITS:
		bcopy(&cnc_kinlimits, (void *)data, sizeof(struct cnc_kinlimits));
		return (0);
	case CNC_SETKINLIMITS:
		bcopy((const void *)data, &cnc_kinlimits, sizeof(struct cnc_kinlimits));
		return (0);
	case CNC_GETTIMINGS:
		bcopy(&cnc_timings, (void *)data, sizeof(struct cnc_timings));
		return (0);
	case CNC_SETTIMINGS:
		bcopy((const void *)data, &cnc_timings, sizeof(struct cnc_timings));
		return (0);
	case CNC_CALTIMINGS:
		if (cnc_timings.hz == 0) {
			cnc_timings.hz = cnc_calibrate_hz();
		}
		if (cnc_timings.move_jog == 0) {
			cnc_timings.move_jog = cnc_calibrate_move_jog();
		}
		bcopy(&cnc_timings, (void *)data, sizeof(struct cnc_timings));
		return (0);
	default:
		break;
	}
	return (ENODEV);
}

#if NSERVO > 0
void
cnc_move_axis(enum cnc_axis axis, int dir)
{
	servo_step(cnc_servos[axis], dir);
	cnc_pos.v[axis] += dir;
}
#endif

/*
 * General-purpose "update" callback. This is invoked periodically during
 * program execution. If it returns -1, the program should abort.
 */
int
cnc_update(void)
{
#if NESTOP > 0
	if (estop_raised()) {
		printf("cnc: e-stop!\n");
		return (-1);
	}
#endif
#if NENCODER > 0
	{
		int i;
		for (i = 0; i < cnc_nencoders; i++)
			encoder_update(cnc_encoders[i]);
	}
#endif
	return (0);
}

#if NSERVO > 0

/*
 * Move a group of servo/stepper driven axes in a coordinated fashion at
 * the specified velocity, subject to our kinematic limits (Amax/Jmax/F).
 *
 * TODO Implement simultaneous acceleration/deceleration of axes at the
 * cost of contour errors, for specialized applications.
 */
int
cnc_move(struct cnc_insn *insn)
{
	struct cnc_quintic_profile Q;
	cnc_real_t t;
	cnc_vec_t v1 = cnc_pos;
	cnc_vec_t v2 = insn->i_pos;
	cnc_vec_t d;
	cnc_pos_t inc, incMin;
	int dir[CNC_NAXES];
	int i, nonzero = 0;
	int axisMajor = 0;
	cnc_pos_t vMajor = 0;

	/*
	 * Compute the difference from the current to the new position,
	 * set axisMajor to the axis with the greatest displacement.
	 */
	for (i = 0; i < CNC_NAXES; i++) {
		d.v[i] = abs(v2.v[i] - v1.v[i]);
		if (d.v[i] > vMajor) {
			vMajor = d.v[i];
			axisMajor = i;
		}
		dir[i] = (v2.v[i] > v1.v[i]) ? 1 : -1;
		if (d.v[i] != 0) { nonzero++; }
	}
	if (!nonzero)
		return (0);		/* Already at target position */

	/*
	 * Rescale the d[] values of the other axes to STEPLEN over the
	 * displacement of axisMajor.
	 */
	for (i = 0; i < CNC_NAXES; i++) {
		if (i != axisMajor)
			d.v[i] = d.v[i]*STEPLEN/d.v[axisMajor];
	}

	/* Compute the time constants for our quintic velocity profile. */
	if (cnc_quintic_init(&Q,
	    (cnc_real_t)d.v[axisMajor],
	    (cnc_real_t)insn->i_v0,
	    (cnc_real_t)insn->i_F,
	    (cnc_real_t)insn->i_Amax,
	    (cnc_real_t)insn->i_Jmax)
	    == -1) {
		goto fail;
	}

	/*
	 * Enter the signal generation loop. We iterate over the displacement
	 * of the major axis.
	 */
	for (inc=0, t=0.0;
	     inc < d.v[axisMajor] && t < MAXSTEPS;
	     inc++, t+=1.0) {
		cnc_real_t v;
		cnc_utime_t delay, j;

		if (cnc_update() == -1)
			goto fail;

		/* Increment the major axis once per iteration. */
		cnc_move_axis(axisMajor, dir[axisMajor]);
		
		/* Increment the other axes by their scaled intervals. */
		for (i = 0; i < CNC_NAXES; i++) {
			if (i != axisMajor) {
				incMin = (dir[i]*d.v[i]*t)/STEPLEN;
				if (v1.v[i]+incMin != cnc_pos.v[i])
					cnc_move_axis(i, dir[i]);
			}
		}

		/*
		 * Evaluate optimal velocity at t in steps/second, and
		 * enter the corresponding delay loop.
		 */
		v = cnc_quintic_step(&Q,
		    t*(Q.Ta + Q.To)/((cnc_real_t)d.v[axisMajor]))*Q.F /
		    (Q.v0 + Q.v1 + Q.v2) +
		    Q.v0;
		if (v > 0) {
			delay = (cnc_utime_t)(cnc_timings.hz/v);
			for (j = 0; j < delay; j++)
				;;
		}
	}
	return (0);
fail:
	printf("cnc: MOVE: Aborted instruction\n");
	return (-1);
}

#if NMPG > 0
/*
 * Manually control the servos with a MPG until estop is raised.
 * The MPG pulses are translated directly to servo pulses.
 */
int
cnc_move_jog_singlestep(struct cnc_insn *insn)
{
	int i;

	if (cnc_nmpgs < 1) {
		printf("cnc: JOG: No MPGs\n");
		return (-1);
	}

	/* Fetch the initial quadrature signal state. */
	for (i = 0; i < cnc_nmpgs; i++)
		mpg_jog_init(cnc_mpgs[i]);

	/*
	 * Loop translating the quadrature signal to directly to step
	 * by step motion.
	 */
	while (!estop_raised()) {
		for (i = 0; i < cnc_nmpgs; i++) {
			struct mpg_softc *mpg = cnc_mpgs[i];
			struct mpg_axis *axis;
			int axisIdx;

			axisIdx = mpg_get_axis(mpg);
			axis = &mpg->sc_axes[axisIdx];
			axis->A = gpio_pin_read(mpg->sc_gpio, &mpg->sc_map, MPG_PIN_A);
			axis->B = gpio_pin_read(mpg->sc_gpio, &mpg->sc_map, MPG_PIN_B);

			if (axis->A == axis->Aprev &&
			    axis->B == axis->Bprev)
				continue;

			if (MPG_TRANSITION_CW(axis)) { cnc_move_axis(axisIdx, 1); }
			if (MPG_TRANSITION_CCW(axis)) { cnc_move_axis(axisIdx, -1); }
			
			axis->Aprev = axis->A;
			axis->Bprev = axis->B;
		}
	}
	return (0);
}

/*
 * Variant of cnc_move() for JOG mode. We use the MPGs to manipulate a
 * desired target position. The routine spins moving the axes to the
 * target position following a quintic velocity profile.
 */
int
cnc_move_jog(struct cnc_insn *insn, int simulate)
{
	struct mpg_softc *mpg;
	struct cnc_quintic_profile Q, Qprev;
	cnc_vec_t vTgt, vTgtLast;
	cnc_real_t t = 0.0, dt = 0.0, L;
	int dir, axis;
	cnc_utime_t Telapsed = 0;
	int moving = 0;
/*	int Tprint1 = 0; */

	/* Initialize the MPG for jog. */
	if (cnc_nmpgs < 1) {
		printf("cnc: JOG: No MPGs\n");
		return (-1);
	}
	mpg = cnc_mpgs[0];		/* TODO allow multiple mpgs */
	mpg_jog_init(mpg);

	/* Initialize the target position. */
	vTgt = cnc_pos;
	vTgtLast = vTgt;
	Q.Ta = 0.0;
	Q.To = 0.0;

	/* Enter the jog loop. */
	while (!estop_raised()) {
		/*
		 * Use the MPG to control the target position, fetch
		 * the currently selected axis.
		 */
		mpg_jog(mpg, &vTgt, insn->i_mult);
		axis = mpg->sc_sel_axis;

		/* Compute direction and distance to target. */
		L = (cnc_real_t)(vTgt.v[axis] - cnc_pos.v[axis]);
		if (L < 0.0) { L = -L; }
		dir = (vTgt.v[axis] > cnc_pos.v[axis]) ? +1 : -1;

#if 0
		if (++Tprint1 > 10000) {
			Tprint1 = 0;
			printf("[t=%s/", cnc_fmt_real(t));
			printf("%s] d=%lld (cnc_pos=%lld)\n",
			    cnc_fmt_real(Q.Ta+Q.To),
			    vTgt.v[axis] - cnc_pos.v[axis],
			    cnc_pos.v[axis]);
		}
#endif
		/*
		 * If the target position has changed, recompute the time
		 * constants and time increment.
		 */
		if (!cnc_vec_same(&vTgtLast, &vTgt)) {
			cnc_real_t t7prev;

			if (!moving) {
				moving = 1;
				if (!simulate) {
					printf("%s: JOG: Moving to %s=%lld\n",
					    ((struct device *)mpg)->dv_xname,
					    cnc_axis_names[axis], vTgt.v[axis]);
					cnc_message("Moving ");
					cnc_message(cnc_axis_names[axis]);
					cnc_message("\n");
				}
			}
			vTgtLast = vTgt;
			Qprev = Q;
			if (cnc_quintic_init(&Q, L,
			    insn->i_v0,
			    insn->i_F,
			    insn->i_Amax,
			    insn->i_Jmax) == -1) {
				goto fail;
			}
			dt = (Q.Ta+Q.To)/L;

			printf("cnc: Quintic: L=%s ", cnc_fmt_real(L));
			printf("dt=%s ", cnc_fmt_real(dt));
			printf("F=%s ", cnc_fmt_real(Q.F));
			printf("v0=%s ", cnc_fmt_real(Q.v0));
			printf("v1=%s ", cnc_fmt_real(Q.v1));
			printf("v2=%s ", cnc_fmt_real(Q.v2));
			printf("Aref=%s ", cnc_fmt_real(Q.Aref));
			printf("[Ts=%s", cnc_fmt_real(Q.Ts));
			printf(" Ta=%s", cnc_fmt_real(Q.Ta));
			printf(" To=%s]\n", cnc_fmt_real(Q.To));

			t = 0.0;
			Telapsed = 0;
			if (t7prev >= 1e-6) {
				printf("cnc: Rescaling t for (Ta+To)=%s: ",
				    cnc_fmt_real(Q.Ta+Q.To));
				t = t/t7prev*(Q.Ta+Q.To); /* Rescale time */
				printf("t=%ss\n", cnc_fmt_real(t));
				Telapsed = 0;
			}
		}

		/*
		 * Move one step to the target position if the velocity profile
		 * has determined that this iteration must move one step.
		 */
		if (moving) {
			if (L > CNC_POS_ERROR) {
				cnc_utime_t v;		/* Steps/second */
				int clipped = 0;

				v = (cnc_utime_t)cnc_quintic_step(&Q, t);
				if (v > (cnc_real_t)cnc_kinlimits.Fmax) {
					v = (cnc_real_t)cnc_kinlimits.Fmax;
					clipped = 1;
				}
				if (v < Q.v0) {
					v = Q.v0;
					clipped = =1;
				}
#if NCNCSTATLED > 0
				cncstatled_set(clipped);
#endif
				/*
				 * Compare the estimated time elapsed (ns)
				 * to the current velocity (steps/sec).
				 */
				if ((Telapsed += cnc_timings.move_jog) >
				    1000000000UL/v) {
					Telapsed = 0;
					if (!simulate) {
						cnc_move_axis(axis, dir);
					} else {
						cnc_pos.v[axis] += dir;
					}
					t += dt;
				}
			} else {
				if (!simulate) {
					printf("%s: JOG: Reached target %s=%lld ",
					    ((struct device *)mpg)->dv_xname,
					    cnc_axis_names[axis], cnc_pos.v[axis]);
					printf("in %s seconds\n", cnc_fmt_real(t));
				}
				t = 0.0;
				moving = 0;
			}
		}

		if (simulate)
			break;
	}
	return (0);
fail:
	printf("cnc: JOG: Aborted instruction\n");
	return (-1);
}
#endif /* NMPG > 0 */

#endif /* NSERVO > 0 */