/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : intest.c
 *      Remarks : Interactive program to test the Kinematic functions.
 *      Usage   : make intest
 */

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


JNTS  jzero = {"          ", 0., 0., 0., 0., 0., 0.};

char *entert();

int sframp;             /* solution angles or range values      */

main()
{
	int exit(), c;

	signal(SIGINT, exit);

	printf("angles/range ?");
	while((c = getchar()) != 'a' && c != 'r') {
	}
	if (c == 'a') {
		sframp = YES;
	}
	else {
		sframp = NO;
	}

	solveconf_n(&jcal_c);
	solveconf_n(&jrng_c);
	solveconf_n(&jzero);

	printf("h / s / j / t / f / i / w / b:");
	while(
(c=getchar())!='h'&&c!='s'&&c!='j'&&c!='t'&&c!='f'&&c!='b'&&c!='i'&&c!='w'){
	}
	switch (c) {
	case 'h' :
		for (; ; ) {
			hdp();          /* handpos      */
		}
	case 's' :
		for (; ; ) {
			sol();          /* solution     */
		}
	case 'j' :
		for (; ; ) {
			jacd();         /* jacobian     */
		}
	case 't' :
		for (; ; ) {
			jact();         /* jacobian T   */
		}
	case 'f' :
		for (; ; ) {
			jacj();         /* give j torques from Cart specs */
		}
	case 'i' :
		for (; ; ) {
			jaci();         /* jacobian -1 (approximation)  */
		}
	case 'w' :
		for (; ; ) {
			jacw();         /* compute the j works in an    */
		}                       /* attempt to study a selection */
	case 'b' :                      /* method                       */
		for (; ; ) {
			jacb();         /* test the current method      */
		}
	}
}


hdp()
{

	TRSF tra, trb;
	TRSF_PTR tma = &tra, tmb = &trb;
	static JNTS jsa = {"          "}  , jsb = {"           "};
	int code;


	enterj(&jsa);
	jns_to_tr_n(tma, &jsa, YES);
	printsc();
	printj(&jsa);
	printr(tma, stdout);
	putchar('\n');
	strcpy(jsb.conf, jsa.conf);
	printj(&jzero);
	printj(&jcal_c);
	printj(&jrng_c);

	code = tr_to_jns_n(&jsb, tma, YES);
	if (code == 01000) {
		printf("wd zero\n");
		return;
	}
	printsc();
	printf("    %c        %c        %c        %c        %c        %c\n",
	(code & 01) ? '1' : '-', (code & 02) ? '2' : '-',
	(code & 04) ? '3' : '-', (code & 010) ? '4' : '-',
	(code & 020) ? '5' : '-', (code & 040) ? '6' : '-');
	jns_to_tr_n(tmb, &jsb, NO);
	printj(&jsb);
	printr(tmb, stdout);
	putchar('\n');
}

sol()
{
	TRSF t6;
	JNTS j6;
	int code;

	j6.conf = entert(&t6);
	printr(&t6, stdout);
	putchar('\n');
	code = tr_to_jns_n(&j6, &t6, YES);
	printsc();
	printj(&jzero);
	printj(&jcal_c);
	printj(&jrng_c);
	printf("    %c        %c        %c        %c        %c        %c\n",
	(code & 01) ? '1' : '-', (code & 02) ? '2' : '-',
	(code & 04) ? '3' : '-', (code & 010) ? '4' : '-',
	(code & 020) ? '5' : '-', (code & 040) ? '6' : '-');
	printj(&j6);
	jns_to_tr_n(&t6, &j6, YES);
	printsc();
	printr(&t6, stdout);
	putchar('\n');
}



jacd()
{
	static JNTS j6 = {"     "},
		    j6d = {"     "},
		    j6D = {"     "},
#ifdef PUMA
		    jd = {"     ", .001, .001, .001, .001, .001, .001};
#endif
#ifdef STAN
		    jd = {"     ", .001, .001,  1., .001, .001, .001};
#endif


	TRSF t6, t6i, t6d, td;
	DIFF d6a, d6b, d6d;

	enterj(&j6);
	printj(&j6);
	putchar('\n');
	fojnts_n(&j6d, &jd, &j6, 1.);
	diffjnts_n(&j6D, &j6d, &j6);
	jns_to_tr_n(&t6, &j6, NO);
	jns_to_tr_n(&t6d, &j6d, YES);
	Trmult(&td, invert(&t6i, &t6), &t6d);
	printd(tr_to_df(&d6a, &td), stdout);
	++rtime;
	jacobD_n(&d6b, &j6D);
	printd(&d6b, stdout);
	diffdiff(&d6d, &d6a, &d6b);
	printd(&d6d, stdout);
	putchar('\n');
}


jact()
{
	static JNTS j6 = {"     "},
		    jt = {"     "};

	TRSF t6;
	FORCE ft6;

	j6.conf = entert(&t6);
	printr(&t6, stdout);
	tr_to_jns_n(&j6, &t6, YES);
	printj(&j6);
	printf(
"- fx fy fz mx my mz ------------------------------------------------------\n"
	   );
	if (scanf("%f%f%f%f%f%f"
	 , &ft6.f.x, &ft6.f.y, &ft6.f.z, &ft6.m.x, &ft6.m.y, &ft6.m.z) == -1)
		exit(0);

	++rtime;
	jacobT_n(&jt, &ft6);
	printt(&jt);
	putchar('\n');
}


jacj()
{
	static JNTS j6 = {"     "},
		    jt = {"     "};

	TRSF t6;
	FORCE ft6;

	printj(&jcal_c);
	enterj(&j6);
	jns_to_tr_n(&t6, &j6, YES);
	printr(&t6, stdout);
	printf(
"- fx fy fz mx my mz ------------------------------------------------------\n"
	   );
	if (scanf("%f%f%f%f%f%f"
	 , &ft6.f.x, &ft6.f.y, &ft6.f.z, &ft6.m.x, &ft6.m.y, &ft6.m.z) == -1)
		exit(0);

	++rtime;
	jacobT_n(&jt, &ft6);
	printt(&jt);
	putchar('\n');
}


jaci()
{
	static JNTS j6 = {"     "}, j6d = {"      "}, j6D = {"     "};
	TRSF t6, t6d, t6D;
	DIFF d6;

	printj(&jcal_c);
	enterj(&j6);
	jns_to_tr_n(&t6, &j6, NO);
	printf("enter diff t6: x y z X Y Z\n");
	scanf("%f%f%f%f%f%f",&d6.t.x, &d6.t.y, &d6.t.z, &d6.r.x, &d6.r.y, &d6.r.z);
	Df_to_tr(&t6D, &d6);
	Trmult(&t6d, &t6, &t6D);
	strcpy(&j6d.conf, &j6.conf);
	tr_to_jns_n(&j6, &t6, YES);
	tr_to_jns_n(&j6d, &t6d, NO);
	diffjnts_n(&j6D, &j6d, &j6);
	printt(&j6D);
	++rtime;
	if (jacobI_n(&j6D, &d6)) printf("degenerate\n");
	printt(&j6D);
}



jacw()
{
	static JNTS j6 = {"      "}, jD;
	TRSF t6;
	TRSF tl, tli;
	DIFF d6, dtl;
	JNTS jt;
	FORCE f6;

	printj(&jcal_c);
	enterj(&j6);
	printf("enter tool\n");
	entert(&tl);
	jns_to_tr_n(&t6, &j6, YES);
	Invert(&tli, &tl);
	for (; ; ) {
		printf("enter diff tl x y z X Y Z\n");
		if (scanf("%f%f%f%f%f%f",
	&dtl.t.x, &dtl.t.y, &dtl.t.z, &dtl.r.x, &dtl.r.y, &dtl.r.z) == -1) {
			return;
		}
		++rtime;
		Difftr(&d6, &dtl, &tli);
		if (jacobI_n(&jD, &d6)) {
			printf("degenerate\n");
			return;
		}
		printf("diff joints :\n");
		printt(&jD);
		Forcetr(&f6, &dtl, &tli);
		jacobT_n(&jt, &f6);
		printf("joint torques\n");
		printt(&jt);
		printf(
"works \n1: %8.1e  2: %8.1e  3: %8.1e  4: %8.1e  5: %8.1e  6: %8.1e\n",
jt.th1*jD.th1,jt.th2*jD.th2,jt.th3*jD.th3,jt.th4*jD.th4,jt.th5*jD.th5,jt.th6*jD.
	}
}





jacb()
{
	static JNTS j6 = {"     "};
	TRSF t6, t;
	VECT k;
	double kx, ky, kz, theta;
	int dirs = 0, choice;
	int sx, sy, sz, sX, sY, sZ;

	t.p.x = t.p.y = t.p.z = 0.;
	printj(&jcal_c);
	enterj(&j6);
	printj(&j6);
	jns_to_tr_n(&t6, &j6, YES);
	printr(&t6, stdout);
	for (; ; ) {
		dirs = 0;
		printf("enter tool rot :kx ky kz theta\n");
		if (scanf("%f%f%f%f", &kx, &ky, &kz, &theta) == -1) {
			return;
		}
		k.x = kx; k.y = ky; k.z = kz;
		Rot(&t, &k, theta);
		printf("comply tool x y z X Y Z\n");
		if (scanf("%d %d %d %d %d %d", &sx, &sy, &sz, &sX, &sY, &sZ) == -1)
			exit(0);

		if (sx) dirs |= SELFX;
		if (sy) dirs |= SELFY;
		if (sz) dirs |= SELFZ;
		if (sX) dirs |= SELMX;
		if (sY) dirs |= SELMY;
		if (sZ) dirs |= SELMZ;

#ifdef PUMA
		GETU5;
		GETH;
#endif
		UPDJ;
		choice = select_n(dirs, &t);
		printf("%o : %c %c %c %c %c %c\n", dirs,
		(choice & SELJ1) ? '1' : ' ',
		(choice & SELJ2) ? '2' : ' ',
		(choice & SELJ3) ? '3' : ' ',
		(choice & SELJ4) ? '4' : ' ',
		(choice & SELJ5) ? '5' : ' ',
		(choice & SELJ6) ? '6' : ' ');
	}
}


enterj(j)
JNTS_PTR j;
{
	double a[6], r[6];

	printf((sframp) ? "--solution angles---" : "--range angles-----");
#ifdef PUMA
	printf("---PUMA-----------------------------\n");
#endif
#ifdef STAN
	printf("---STANFORD-ARM---------------------\n");
#endif
	if (scanf("%f%f%f%f%f%f"
	 , &a[0], &a[1], &a[2], &a[3], &a[4], &a[5]) == -1)
		exit(0);

	a[0] *= dgtord_m;
	a[1] *= dgtord_m;
#ifdef PUMA
	a[2] *= dgtord_m;
#endif
	a[3] *= dgtord_m;
	a[4] *= dgtord_m;
	a[5] *= dgtord_m;


	if (sframp) {
		angtorng(r, a);
		j->th1 = r[0];
		j->th2 = r[1];
		j->th3 = r[2];
		j->th4 = r[3];
		j->th5 = r[4];
		j->th6 = r[5];
	}
	else {
		j->th1 = a[0];
		j->th2 = a[1];
		j->th3 = a[2];
		j->th4 = a[3];
		j->th5 = a[4];
		j->th6 = a[5];
	}
}

char *entert(t)
TRSF_PTR t;
{

	static char conf[10];
	double x, y, z, kx, ky, kz;
	VECT k;
	real theta;

	printf("- x  y  z --- kx ky kz - theta - conf ");
#ifdef PUMA
	printf("---PUMA-----------------------------\n");
#endif
#ifdef STAN
	printf("---STANFORD-ARM---------------------\n");
#endif

	if ((scanf("%f%f%f%f%f%f%f%s"
	   , &x, &y, &z, &kx, &ky, &kz, &theta, conf)) == -1)
		exit(0);
	k.x = kx;
	k.y = ky;
	k.z = kz;
	Rot(t, &k, theta);
	Trsl(t, x, y, z);
	return(conf);
}



printj(j)
JNTS_PTR j;
{
	double a[6], r[6];

	if (sframp) {
		r[0] = j->th1;
		r[1] = j->th2;
		r[2] = j->th3;
		r[3] = j->th4;
		r[4] = j->th5;
		r[5] = j->th6;
		rngtoang(a, r);
	}
	else {
		a[0] = j->th1;
		a[1] = j->th2;
		a[2] = j->th3;
		a[3] = j->th4;
		a[4] = j->th5;
		a[5] = j->th6;
	}

	printf("%8.3f %8.3f %8.3f %8.3f %8.3f %8.3f  %s %s\n",
	a[0] * rdtodg_m,
	a[1] * rdtodg_m,
#ifdef PUMA
	a[2] * rdtodg_m,
#endif
#ifdef STAN
	a[2],
#endif
	a[3] * rdtodg_m,
	a[4] * rdtodg_m,
	a[5] * rdtodg_m,
	j->conf,
	(sframp) ? "   (sol ang)" : "   (rng ang)");
}


printsc()
{
#ifdef PUMA
	if (fabs((sncs_d.s1 * sncs_d.s1 + sncs_d.c1 * sncs_d.c1) - 1) > SMALL) {
		printf("J1 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s2 * sncs_d.s2 + sncs_d.c2 * sncs_d.c2) - 1) > SMALL) {
		printf("J2 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s23 * sncs_d.s23 + sncs_d.c23 * sncs_d.c23) - 1) > SMALL) {
		printf("J23 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s3 * sncs_d.s3 + sncs_d.c3 * sncs_d.c3) - 1) > SMALL) {
		printf("J3 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s4 * sncs_d.s4 + sncs_d.c4 * sncs_d.c4) - 1) > SMALL) {
		printf("J4 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s5 * sncs_d.s5 + sncs_d.c5 * sncs_d.c5) - 1) > SMALL) {
		printf("J5 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s6 * sncs_d.s6 + sncs_d.c6 * sncs_d.c6) - 1) > SMALL) {
		printf("J6 : bad sin cos------------------------\n");
	}
#endif
#ifdef STAN
	if (fabs((sncs_d.s1 * sncs_d.s1 + sncs_d.c1 * sncs_d.c1) - 1) > SMALL) {
		printf("J1 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s2 * sncs_d.s2 + sncs_d.c2 * sncs_d.c2) - 1) > SMALL) {
		printf("J2 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s4 * sncs_d.s4 + sncs_d.c4 * sncs_d.c4) - 1) > SMALL) {
		printf("J4 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s5 * sncs_d.s5 + sncs_d.c5 * sncs_d.c5) - 1) > SMALL) {
		printf("J5 : bad sin cos------------------------\n");
	}
	if (fabs((sncs_d.s6 * sncs_d.s6 + sncs_d.c6 * sncs_d.c6) - 1) > SMALL) {
		printf("J6 : bad sin cos------------------------\n");
	}
#endif
}


diffdiff(r, a, b)
DIFF_PTR r, a, b;
{
	r->t.x = a->t.x - b->t.x;
	r->t.y = a->t.y - b->t.y;
	r->t.z = a->t.z - b->t.z;
	r->r.x = a->r.x - b->r.x;
	r->r.y = a->r.y - b->r.y;
	r->r.z = a->r.z - b->r.z;
}


printt(d)
JNTS_PTR d;
{
	printf(
	"1: %8.1e  2: %8.1e  3: %8.1e  4: %8.1e  5: %8.1e  6: %8.1e\n",
	d->th1, d->th2, d->th3, d->th4, d->th5, d->th6);
}
