/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : misc.c
 *      Remarks : Do all the io. Very dependent on the version.
 *                Also low level interface.
 *      Usage   : part of the library
 */

#include <signal.h>
#include "../h/which.h"
#include "../h/switch.h"
#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/kine.h"
#include "../h/bio.h"
#include "../h/umac.h"


#ifdef REAL
#include "../h/rtc.h"

extern struct chg chg;                  /* in rtc                       */
extern struct how how;                  /* .. ...                       */

static unsigned short old[NJOINTS] =    /* previous encoder values      */
		{0, 0, 0, 0, 0, 0};
static JNTS lod;                        /* gravity loads                */


/*
 * obtain observed joint torques.
 * if velocity small : no torque, otherwise we get very erroneous results
 * remove gravity loads
 */

getobst_n(j) /*::*/
register JNTS_PTR j;
{
	double t[NJOINTS];              /* observed torques             */

	adctotor(t, how.adcr, how.pos, old);
	if (ABS((short)(how.pos[J1] - old[J1])) < 1) {
		j->th1 = 0.;
	}
	else {
		j->th1 = lod.th1 - t[J1];
	}
	if (ABS((short)(how.pos[J2] - old[J2])) < 1) {
		j->th2 = 0.;
	}
	else {
		j->th2 = lod.th2 - t[J2];
	}
	if (ABS((short)(how.pos[J3] - old[J3])) < 1) {
		j->th3 = 0.;
	}
	else {
		j->th3 = lod.th3 - t[J3];
	}
	if (ABS((short)(how.pos[J4] - old[J4])) < 1) {
		j->th4 = 0.;
	}
	else {
		j->th4 = lod.th4 - t[J4];
	}
	if (ABS((short)(how.pos[J5] - old[J5])) < 1) {
		j->th5 = 0.;
	}
	else {
		j->th5 = lod.th5 - t[J5];
	}
	if (ABS((short)(how.pos[J6] - old[J6])) < 1) {
		j->th6 = 0.;
	}
	else {
		j->th6 = lod.th6 - t[J6];
	}
}


/*
 * obtain observed joint pos
 */

getobsj_n(j) /*::*/
register JNTS_PTR j;
{
	double r[NJOINTS];

	enctorng(r, how.pos);
	j->th1 = r[J1];
	j->th2 = r[J2];
	j->th3 = r[J3];
	j->th4 = r[J4];
	j->th5 = r[J5];
	j->th6 = r[J6];
}


/*
 * sort out the requests of setpoint and translate them for rtc
 * add gravity loads
 * returns 'LIMIT' if next to limits
 */

jnsend_n(j, s, cpy, t) /*:: must be called the last */
register JNTS_PTR j;
register int s;
register int cpy;
register JNTS_PTR t;
{
	double jset[NJOINTS];           /* desired setpoint     */
	double jtor[NJOINTS];           /* desired torques      */
	unsigned short enc[NJOINTS];    /* encoder values       */
	short cur[NJOINTS];             /* current values       */
	int code = OK;

#ifdef PUMA
	gravload_n(&lod,
sncs_d.c2, sncs_d.c23, sncs_d.s23, sncs_d.c4, sncs_d.s4, sncs_d.c5, sncs_d.s5);
#endif
#ifdef STAN
	gravload_n(&lod,
sncs_d.s2, sncs_d.c2, sncs_d.d3, sncs_d.c4, sncs_d.s4, sncs_d.c5, sncs_d.s5);
#endif

	jset[J1] = j->th1;
	jset[J2] = j->th2;
	jset[J3] = j->th3;
	jset[J4] = j->th4;
	jset[J5] = j->th5;
	jset[J6] = j->th6;

	if (rngtoenc(enc, jset)) {
		code = LIMIT;
	}

	jtor[J1] = t->th1 + lod.th1;
	jtor[J2] = t->th2 + lod.th2;
	jtor[J3] = t->th3 + lod.th3;
	jtor[J4] = t->th4 + lod.th4;
	jtor[J5] = t->th5 + lod.th5;
	jtor[J6] = t->th6 + lod.th6;

	tortodac(cur, jtor, how.pos, old);

	if (cpy & SELJ1) {
		chg.i_motion[J1].vali = cur[J1];
		chg.i_motion[J1].set = CUR;
	}
	else {
		chg.i_motion[J1].vali = enc[J1];
		chg.i_motion[J1].set = POS;
	}
	if (cpy & SELJ2) {
		chg.i_motion[J2].vali = cur[J2];
		chg.i_motion[J2].set = CUR;
	}
	else {
		chg.i_motion[J2].vali = enc[J2];
		chg.i_motion[J2].set = POS;
	}
	if (cpy & SELJ3) {
		chg.i_motion[J3].vali = cur[J3];
		chg.i_motion[J3].set = CUR;
	}
	else {
		chg.i_motion[J3].vali = enc[J3];
		chg.i_motion[J3].set = POS;
	}
	if (cpy & SELJ4) {
		chg.i_motion[J4].vali = cur[J4];
		chg.i_motion[J4].set = CUR;
	}
	else {
		chg.i_motion[J4].vali = enc[J4];
		chg.i_motion[J4].set = POS;
	}
	if (cpy & SELJ5) {
		chg.i_motion[J5].vali = cur[J5];
		chg.i_motion[J5].set = CUR;
	}
	else {
		chg.i_motion[J5].vali = enc[J5];
		chg.i_motion[J5].set = POS;
	}
	if (cpy & SELJ6) {
		chg.i_motion[J6].vali = cur[J6];
		chg.i_motion[J6].set = CUR;
	}
	else {
		chg.i_motion[J6].vali = enc[J6];
		chg.i_motion[J6].set = POS;
	}

	if (hdpos != 0) {
		chg.g_hand.valg = hdpos;
		chg.g_hand.set = YES;
		hdpos = 0;
	}
	if (s != 0) {
		double fact = s / DEF_SAMPLE;
		register int n, r;

		jmxv_c.th1 *= fact;
		jmxv_c.th2 *= fact;
		jmxv_c.th3 *= fact;
		jmxv_c.th4 *= fact;
		jmxv_c.th5 *= fact;
		jmxv_c.th6 *= fact;
		n = s / HARDCLOCK;
		r = 0;
		while ((n >>= 1) != 0) {
			++r;
		}
		chg.g_rate.valg = r;
		chg.g_rate.set = YES;
	}

	old[J1] = how.pos[J1];
	old[J2] = how.pos[J2];
	old[J3] = how.pos[J3];
	old[J4] = how.pos[J4];
	old[J5] = how.pos[J5];
	old[J6] = how.pos[J6];

	return(code);
}
#endif


#ifndef REAL            /* display facilities */

static putenco(enc, j) /*##*/
register unsigned short *enc;
JNTS_PTR j;
{
	double jset[NJOINTS];

	jset[J1] = j->th1;
	jset[J2] = j->th2;
	jset[J3] = j->th3;
	jset[J4] = j->th4;
	jset[J5] = j->th5;
	jset[J6] = j->th6;
 (void) rngtoenc(enc, jset);
	enc[6] = hdpos << 4;            /* that's for play      */
}


/*
 * simulation cannot do anything
 */

getobst_n(j) /*::*/
JNTS_PTR j;
{
	j->th1 = 0.;
	j->th2 = 0.;
	j->th3 = 0.;
	j->th4 = 0.;
	j->th5 = 0.;
	j->th6 = 0.;
}


/*
 * simulates comply by blocking the joints
 */

static JNTS simulcpyj;

getobsj_n(j) /*::*/
JNTS_PTR j;
{
	assignjs_n(j, &simulcpyj);
}


/*
 * catches the INT signal
 */

static enough() /*##*/
{
	mess = "Interrupt";
	terminate = YES;
}


/*
 * displays t6 and joints
 */

#define BOUND   5. * DEGTORAD

jnsend_n(p, j, jl, t, ts, tm, cpyj, mt) /*::*/
PST_PTR p;
JNTS_PTR j, jl;
TRSF_PTR t;
int ts, tm;
int cpyj;
char mt;
{
	unsigned short enco[7];
	int code = OK;
	real bound = BOUND;

 (void) signal(SIGINT, enough);

	if (!(cpyj & SELJ1)) {
		simulcpyj.th1 = j->th1;
	}
	if (!(cpyj & SELJ2)) {
		simulcpyj.th2 = j->th2;
	}
	if (!(cpyj & SELJ3)) {
		simulcpyj.th3 = j->th3;
	}
	if (!(cpyj & SELJ4)) {
		simulcpyj.th4 = j->th4;
	}
	if (!(cpyj & SELJ5)) {
		simulcpyj.th5 = j->th5;
	}
	if (!(cpyj & SELJ6)) {
		simulcpyj.th6 = j->th6;
	}

	if (fabs(j->th1 - jl->th1) > jmxv_c.th1 ||
	    fabs(j->th2 - jl->th2) > jmxv_c.th2 ||
	    fabs(j->th3 - jl->th3) > jmxv_c.th3 ||
	    fabs(j->th4 - jl->th4) > jmxv_c.th4 ||
	    fabs(j->th5 - jl->th5) > jmxv_c.th5 ||
	    fabs(j->th6 - jl->th6) > jmxv_c.th6) {
		fprintf(stderr, "glitch %d\n", rtime);
	}
	if (j->th1 < bound || j->th1 > jrng_c.th1 - bound ||
	    j->th2 < bound || j->th2 > jrng_c.th2 - bound ||
	    j->th3 < bound || j->th3 > jrng_c.th3 - bound ||
	    j->th4 < bound || j->th4 > jrng_c.th4 - bound ||
	    j->th5 < bound || j->th5 > jrng_c.th5 - bound ||
	    j->th6 < bound || j->th6 > jrng_c.th6 - bound) {
		fprintf(stderr, "limit  %d\n", rtime);
		code = LIMIT;
	}

	if (opsw_n.encoders) {
		putenco(enco, j);
		Write(&iobf_n[EB], (char *)enco, sizeof(enco));
	}
	if (opsw_n.t6butnotj6) {
		if (opsw_n.graphics)
			outmat(t, tm, mt);
		if (opsw_n.numerics)
			prtmat(p, t, ts, tm, mt, fpo);
	}
	else {
		if (opsw_n.graphics)
			outjns(&simulcpyj, tm, mt);
		if (opsw_n.numerics)
			prtjns(p, &simulcpyj, ts, tm, mt, cpyj, fpo);
	}
 (void) signal(SIGINT, release_l);
	return(code);
}

/*
 * output t6 in a buffered manner
 */

static outmat(t, tm, mt) /*##*/
TRSF_PTR t;
int tm;
char mt;
{
	double r;

	Write(&iobf_n[0], (r = t->p.x, (char *)&r), sizeof(double));
	Write(&iobf_n[1], (r = t->p.y, (char *)&r), sizeof(double));
	Write(&iobf_n[2], (r = t->p.z, (char *)&r), sizeof(double));
	Write(&iobf_n[3], (r = t->n.x, (char *)&r), sizeof(double));
	Write(&iobf_n[4], (r = t->n.y, (char *)&r), sizeof(double));
	Write(&iobf_n[5], (r = t->n.z, (char *)&r), sizeof(double));
	Write(&iobf_n[6], (r = t->o.x, (char *)&r), sizeof(double));
	Write(&iobf_n[7], (r = t->o.y, (char *)&r), sizeof(double));
	Write(&iobf_n[8], (r = t->o.z, (char *)&r), sizeof(double));
	Write(&iobf_n[9], (r = t->a.x, (char *)&r), sizeof(double));
	Write(&iobf_n[10], (r = t->a.y, (char *)&r), sizeof(double));
	Write(&iobf_n[11], (r = t->a.z, (char *)&r), sizeof(double));
	Write(&iobf_n[TB], (char *)&tm, sizeof(int));
	Write(&iobf_n[CB], (char *)&mt, sizeof(char));
}

static double rngp(a) /*##*/
double a;
{
	while(a <= -pi_m)
		a += pit2_m;
	while(a > pi_m)
		a -= pit2_m;
	return(a);
}


/*
 * output j6 in a buffered manner
 */

static outjns(j, tm, mt) /*##*/
JNTS_PTR j;
{
	double r;

	Write(&iobf_n[J1],
		(r = ((opsw_n.angles) ? rngp(j->th1 + jmin_c.th1) : j->th1)
		* rdtodg_m, (char *)&r), sizeof(double));
	Write(&iobf_n[J2],
		(r = ((opsw_n.angles) ? rngp(j->th2 + jmin_c.th2) : j->th2)
		* rdtodg_m, (char *)&r), sizeof(double));
#ifdef PUMA
	Write(&iobf_n[J3],
		(r = ((opsw_n.angles) ? rngp(j->th3 + jmin_c.th3) : j->th3)
		* rdtodg_m, (char *)&r), sizeof(double));
#endif
#ifdef STAN
	Write(&iobf_n[J3],
		(r = ((opsw_n.angles) ? j->th3 + jmin_c.th3 : j->th3),
		(char *)&r), sizeof(double));
#endif
	Write(&iobf_n[J4],
		(r = ((opsw_n.angles) ? rngp(j->th4 + jmin_c.th4) : j->th4)
		* rdtodg_m, (char *)&r), sizeof(double));
	Write(&iobf_n[J5],
		(r = ((opsw_n.angles) ? rngp(j->th5 + jmin_c.th5) : j->th5)
		* rdtodg_m, (char *)&r), sizeof(double));
	Write(&iobf_n[J6],
		(r = ((opsw_n.angles) ? rngp(j->th6 + jmin_c.th6) : j->th6)
		* rdtodg_m, (char *)&r), sizeof(double));
	Write(&iobf_n[TB], (char *)&tm, sizeof(int));
	Write(&iobf_n[CB], (char *)&mt, sizeof(char));
}


/*
 * print t6 on @.out
 */

static prtmat(p, t, ts, tm, mt, fpt) /*##*/
PST_PTR p;
TRSF_PTR t;
int ts, tm;
char mt;
FILE *fpt;
{
	fprintf(fpt, "%8s ", p->name);
	fprintf(fpt, "%c %4d %4d ", mt, tm, ts);
	fprintf(fpt,
"%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
	t->p.x, t->p.y, t->p.z,
	t->n.x, t->n.y, t->n.z,
	t->o.x, t->o.y, t->o.z,
	t->a.x, t->a.y, t->a.z);
}


/*
 * print j6 on @.out
 */

static prtjns(p, j, ts, tm, mt, cpyj, fpt) /*##*/
PST_PTR p;
JNTS_PTR j;
int ts, tm;
char mt;
int cpyj;
FILE *fpt;
{
	fprintf(fpt, "%8s ", p->name);
	fprintf(fpt, "%c %4d %4d ", mt, tm, ts);
	fprintf(fpt, "%7.2f %7.2f %7.2f %7.2f %7.2f %7.2f %s %o\n",
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th1 + jmin_c.th1) : j->th1),
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th2 + jmin_c.th2) : j->th2),
#ifdef PUMA
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th3 + jmin_c.th3) : j->th3),
#endif
#ifdef STAN
	((opsw_n.angles) ? j->th3 + j->th3 : jmin_c.th3),
#endif
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th4 + jmin_c.th4) : j->th4),
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th5 + jmin_c.th5) : j->th5),
	rdtodg_m * ((opsw_n.angles) ? rngp(j->th6 + jmin_c.th6) : j->th6),
	j->conf, cpyj);
}

/*
 * buffered writes, to avoid too many syscall
 */

static Write(b, d, s) /*##*/
BIO_PTR b;
char *d;
int s;
{
	if (b->ptr == NULL) {
		b->ptr = b->buf;
	}
	while (s--) {
		*(b->ptr)++ = *d++;
		if (b->ptr - b->buf == BUFS) {
			if (write(b->fd, b->buf, BUFS) != BUFS) {
				fprintf(stderr, "Write io error\n");
				exit(2);
			}
			b->ptr = b->buf;
		}
	}
}
#endif /* not REAL */
