/*	$OpenBSD$	*/

/*
 * Copyright (c) 2007 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 "cnc_devicevar.h"
#include "cnc_servovar.h"
#include "cnc_estopvar.h"
#include "cnc_spindlevar.h"
#include "cncvar.h"

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

cnc_real_t cnc_Vmax = 2000.0;		/* maximum velocity (steps/sec) */
u_long cnc_timebase = 0;		/* delay loop calibration */

const char *cnc_insn_names[] = {
	"MOVE",
	"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_device *cnc_servos[CNC_NAXES];
struct cnc_device *cnc_spindles[CNC_MAX_SPINDLES];
struct cnc_device *cnc_estops[CNC_MAX_ESTOPS];
struct cnc_device *cnc_estop;
int cnc_nservos = 0;
int cnc_nspindles = 0;
int cnc_nestops = 0;

void
cncattach(int num)
{
	int i;

	if (num > 1)
		return;

	printf("cnc: STEPLEN=%u, Vmax = %d steps/s\n",
	    (u_int)STEPLEN, (int)cnc_Vmax);
	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;
}

int
cncdetach(struct device *self, int flags)
{
	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);
}

struct cnc_device *
cnc_get_spindle(int id)
{
	if (id < 0 || id >= cnc_nspindles) {
		return (NULL);
	}
	return (cnc_spindles[id]);
}

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

	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) {
		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_SET_INTERP:
			if (insn.i_interp_mode < 0 ||
			    insn.i_interp_mode >= CNC_INTERP_LAST) {
				cause = "bad interpolation mode";
				goto fail;
			}
			break;
		case CNC_SPINDLE_DIR:
			if (cnc_get_spindle(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:
			if ((p = cnc_get_spindle(insn.i_spindle_id)) == NULL) {
				cause = "no such spindle";
				goto fail;
			} else {
				struct spindle_softc *sc = p;

				if (insn.i_spindle_speed > sc->sc_speed_max) {
					cause = "speed exceeds spindle limit";
					goto fail;
				}
			}
			break;
		case CNC_SPINDLE_START:
		case CNC_SPINDLE_STOP:
			if ((p = cnc_get_spindle(insn.i_spindle_id)) == NULL) {
				cause = "no such spindle";
				goto fail;
			}
			break;
		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 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); */
}

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

	if (cnc_nservos != CNC_NAXES) {
		printf("cnc: nservos=%d, but CNC_NAXES=%d\n", cnc_nservos,
		    CNC_NAXES);
		return (EIO);
	}

	printf("cnc: executing program\n");

	s = splclock();
	TAILQ_FOREACH(insn, &cnc_prog, prog) {
		switch (insn->i_type) {
		case CNC_MOVE:
			if (cnc_move(insn) == -1) {
				goto fail;
			}
			break;
#if NSPINDLE > 0
		case CNC_SPINDLE_START:
			spindle_start(cnc_spindles[insn->i_spindle_id], insn);
			break;
		case CNC_SPINDLE_STOP:
			spindle_stop(cnc_spindles[insn->i_spindle_id], insn);
			break;
		case CNC_SPINDLE_SPEED:
			spindle_speed(cnc_spindles[insn->i_spindle_id], insn);
			break;
#endif
		default:
			break;
		}
		printf("insn %u: type=%d\n", ninsns, insn->i_type);
		ninsns++;
	}
	splx(s);
	return (0);
fail:
	splx(s);
	return (EIO);
}

/*
 * Try to calibrate cnc_timebase to a value that will give us as
 * close as possible to 1 second. There is no portable way to do
 * this so we repeatedly benchmark results of a real delay loop
 * until the error is reduced to a minimum.
 *
 * This can take a while, but only needs to be run once on any
 * given machine.
 *
 * XXX the algorithm should be optimized based on the rate of change
 * in the results.
 */
u_long
cnc_calibrate_timebase(void)
{
	struct timeval tv1, tv2;
	long ds, du, d;
	u_long timebase = 10000000, i;
	int s, j;

	s = splhigh();
	printf("cnc: calibrating timebase...");
	for (j = 0; j < 500; j++) {
		microtime(&tv1);
		for (i = 0; i < timebase; i++)
			;;
		microtime(&tv2);
		ds = tv2.tv_sec - tv1.tv_sec;
		du = tv2.tv_usec - tv1.tv_usec;

		printf(" %ld", 1000000-du);
		if (ds < 0) {
			timebase += 1000000;
		} else if (ds > 0) {
			timebase -= 1000000;
		} else if (ds == 0 && du < 1000000) {
			d = 1000000 - du;
			timebase += d*4;
			if (d < 10) { break; }
		} else if (ds == 0 && du > 1000000) {
			d = du - 1000000;
			timebase -= d*4;
			if (d < 10) { break; }
		} else {
			break;
		}
	}
	splx(s);
	printf("\n");
	return (timebase);
}

int
cncioctl(dev_t dev, u_long cmd, caddr_t data, int flag, struct proc *p)
{
	struct cnc_kinematics *kin;
	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: axis %d: %lu -> %lu", 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_GETKINLIMITS:
		kin = (struct cnc_kinematics *)data;
//		kin->k_Ts = (int)cnc_Ts;
		kin->k_Vmax = (int)cnc_Vmax;
		return (0);
	case CNC_SETKINLIMITS:
		kin = (struct cnc_kinematics *)data;
		if (kin->k_Ts < 0 || kin->k_Vmax < 0) {
			return (EINVAL);
		}
//		cnc_Ts = (cnc_real_t)kin->k_Ts;
		cnc_Vmax = (cnc_real_t)kin->k_Vmax;
		return (0);
	case CNC_GETTIMEBASE:
		*(u_long *)data = cnc_timebase;
		return (0);
	case CNC_SETTIMEBASE:
		cnc_timebase = *(u_long *)data;
		return (0);
	case CNC_CALTIMEBASE:
		*(u_long *)data = cnc_calibrate_timebase();
		return (0);
	default:
		break;
	}
	return (ENODEV);
}

void
cnc_move_axis(enum cnc_axis axis, int dir)
{
	servo_step(cnc_servos[axis], dir);
	cnc_pos.v[axis] += dir;
}

/* Check whether an e-stop device is active. */
int
cnc_estop_raised(void)
{
	int i;

	for (i = 0; i < cnc_nestops; i++) {
		if (estop_get_state(cnc_estops[i])) {
			printf("%s: emergency stop\n",
			    cnc_estops[i]->cd_dev.dv_xname);
			return (1);
		}
	}
	return (0);
}

/*
 * Coordinated movement of the n axes to a target position in n-dimensional
 * space at the specified feed rate.
 *
 * TODO Specialized applications could use another algorithm to provide a
 * more constant velocity through simultaneous deceleration and acceleration
 * of the axes, at the cost of contour errors proportional to Ts.
 */
int
cnc_move(struct cnc_insn *insn)
{
	struct cnc_quintic_profile qprof;
	cnc_real_t t;
#if CNC_NAXES == 3
	cnc_pos_t x1 = cnc_pos.v[CNC_X];
	cnc_pos_t y1 = cnc_pos.v[CNC_Y];
	cnc_pos_t z1 = cnc_pos.v[CNC_Z];
	cnc_pos_t x2 = insn->i_pos.v[CNC_X];
	cnc_pos_t y2 = insn->i_pos.v[CNC_Y];
	cnc_pos_t z2 = insn->i_pos.v[CNC_Z];
	cnc_pos_t dx, dy, dz;
	cnc_pos_t x, y, z;
	int xdir, ydir, zdir;
#endif

#if CNC_NAXES == 3
	dx = abs(x2 - x1);
	dy = abs(y2 - y1);
	dz = abs(z2 - z1);

	if (dx == 0 && dy == 0 && dz == 0)
		return (0);

	xdir = (x2 > x1) ? 1 : -1;
	ydir = (y2 > y1) ? 1 : -1;
	zdir = (z2 > z1) ? 1 : -1;

	printf("MOVE: Delta=%ld,%ld,%ld\n", dx, dy, dz);
	printf("MOVE: Dir=%d,%d,%d\n", xdir, ydir, zdir);
	printf("MOVE: F = %d st/s, Amax = %d st/s^2, Jmax = %d st/s^3\n",
	    insn->i_F, insn->i_Amax, insn->i_Jmax);

	if (dx >= dy && dx >= dz) {
		dy = dy*STEPLEN/dx;
		dz = dz*STEPLEN/dx;
		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dx) == -1) {
			goto fail;
		}
		for (x = 0, t = 0.0;
		     x < dx && t < MAXSTEPS;
		     x++, t+=1.0) {
			y = (dy*ydir*t)/STEPLEN;
			z = (dz*zdir*t)/STEPLEN;
			if (cnc_estop_raised()) {
				goto fail;
			}
			cnc_move_axis(CNC_X, xdir);
			if (y1+y != cnc_pos.v[CNC_Y]) {
				cnc_move_axis(CNC_Y, y);
				cnc_pos.v[CNC_Y] = y1+y;
			}
			if (z1+z != cnc_pos.v[CNC_Z]) {
				cnc_move_axis(CNC_Z, z);
				cnc_pos.v[CNC_Z] = z1+z;
			}
			cnc_move_delay(&qprof, t, (cnc_real_t)dx);
		}
	} else if (dy >= dx && dy >= dz) {
		dx = dx*STEPLEN/dy;
		dz = dz*STEPLEN/dy;
		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dy) == -1) {
			goto fail;
		}
		for (y = 0, t = 0.0;
		     y < dy && t < MAXSTEPS;
		     y++, t+=1.0) {
			x = (dx*xdir*t)/STEPLEN;
			z = (dz*zdir*t)/STEPLEN;
			if (cnc_estop_raised()) {
				goto fail;
			}
			cnc_move_axis(CNC_Y, ydir);
			if (x1+x != cnc_pos.v[CNC_X]) {
				cnc_move_axis(CNC_X, x);
				cnc_pos.v[CNC_X] = x1+x;
			}
			if (z1+z != cnc_pos.v[CNC_Z]) {
				cnc_move_axis(CNC_Z, z);
				cnc_pos.v[CNC_Z] = z1+z;
			}
			cnc_move_delay(&qprof, t, (cnc_real_t)dy);
		}
	} else if (dz >= dx && dz >= dy) {
		dx = dx*STEPLEN/dz;
		dy = dy*STEPLEN/dz;
		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dz) == -1) {
			goto fail;
		}
		for (z = 0, t = 0.0;
		     z < dz && t < MAXSTEPS;
		     z++, t+=1.0) {
			x = (dx*xdir*t)/STEPLEN;
			y = (dy*ydir*t)/STEPLEN;
			if (cnc_estop_raised()) {
				goto fail;
			}
			cnc_move_axis(CNC_Z, zdir);
			if (x1+x != cnc_pos.v[CNC_X]) {
				cnc_move_axis(CNC_X, x);
				cnc_pos.v[CNC_X] = x1+x;
			}
			if (y1+y != cnc_pos.v[CNC_Y]) {
				cnc_move_axis(CNC_Y, y);
				cnc_pos.v[CNC_Y] = y1+y;
			}
			cnc_move_delay(&qprof, t, (cnc_real_t)dz);
		}
	} else {
		printf("MOVE: Bad case\n");
		goto fail;
	}
#endif
	return (0);
fail:
	printf("MOVE: Aborted instruction\n");
	return (-1);
}

/*
 * Enter a precisely-calculated loop that will give us the proper delay
 * between two steps.
 */
void
cnc_move_delay(const struct cnc_quintic_profile *q, cnc_real_t t, cnc_real_t L)
{
	cnc_real_t v;
	u_long delay;
	u_long j;

	v = cnc_quintic_step(q, t*(q->Ta + q->To)/L)*q->F/(q->v0+q->v1+q->v2) +
	    q->v0;
	delay = (u_long)(cnc_timebase/v);
	if (delay > cnc_timebase) {
		delay = cnc_timebase;
	}
#if 1
	for (j = 0; j < delay; j++)
		;;
#else
	printf("[%lu] %s steps/sec (delay=%lu)\n", (u_long)t, cnc_fmt_real(v),
	    delay);
#endif
}

