#include "../h/rccl.h"
#include "../h/rtc.h"
#include "../h/hand.h"
#include "../h/umac.h"

extern struct how how;
extern struct chg chg;

#define CLR     '\032'

pumatask()
{
	int q;

	putchar(CLR);
	printf("This is an implementation of the program given in Chap 5.\n");
	printf("of Prof. Paul's book\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		book();
	}
	putchar(CLR);
	printf("This program demonstrates that we can perform a task\n");
	printf("namely, moving in loop to four different locations,\n");
	printf("with respect to a moving coordinate frame.\n");
	printf("The first part is performed in cartesian mode\n");
	printf("in other words, the trajectory is constantly evaluated\n");
	printf("with respect to the Cartesian world description\n");
	printf("The second part is executed in joint mode,\n");
	printf("this means that the succesive locations are computed\n");
	printf("only at the beginning of each path segment, the coordinate\n");
	printf("velocities estimated, and the intermediate locations\n");
	printf("linearly interpolated at the joint level.\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		cvsj1();
	}
	putchar(CLR);
	printf("This example is similar to the previous one.\n");
	printf("Now the moving coordinate frame describes a circle\n");
	printf("and the same move statement is executed four times.\n");
	printf("This example clearly shows that the trajectories executed\n");
	printf("in joint mode are aproximate\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		cvsj2();
	}
	putchar(CLR);
	printf("This example again uses of moving coordinate frame.\n");
	printf("We will instruct the robot to move along a path defined\n");
	printf("by a spiral and a rotation\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		spire();
	}
	putchar(CLR);
	printf("This example again uses of moving coordinate frame.\n");
	printf("We will instruct the robot to move at separate locations\n");
	printf("by changing asynchronously the value of a frame\n");
	printf("as if this frame was updated by some external sensor\n");
	printf("like a camera or a range finder.\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		psv();
	}
	putchar(CLR);
	printf("This program makes use a very simple sensor to perform\n");
	printf("a trajectory tracking on an arbitrary surface\n\n");
	printf("The second part of the program uses the same sensor in a different way.
	printf("Motions will be interrupted upon detection of an obstacle\n");
	printf("When the robot senses an obtacle, it proceeds to the next\n");
	printf("path segment.\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		ftrk();
	}
	putchar(CLR);
	printf("This program shows the problems caused by the volume\n");
	printf("swept when the arm changes it's configuration.\n");
	printf("Type 'y' to proceed");
	QUERY(q);
	if (q == 'y') {
		pcnf();
	}
	putchar(CLR);
}


book()
{

	TRSF_PTR e, z, p, pa, pg, pd, h, ht, pha, pch, pac, pn;
	POS_PTR pos1, pos2, pos3, pos4, pos5, pos6, pos7, pos8;
	int q;

	z = gentr_eul("Z", 0., 0., 500., 0., 0., 0.);
	e = gentr_eul("E", 0., 0., 200., 0., 0., 0.);
	p    = gentr_eul("P"   , 200., 0., 0., -90., -90., 0.);
	pa   = gentr_eul("PA"  , 30., 0., -50., 0., -45., 0.);
	pg   = gentr_eul("PG"  , 0., 0., -50., 0., -45., 0.);
	pd   = gentr_eul("PD"  , 50., 0., 0., 0., 0., 0.);
	h    = gentr_eul("H"   , 250., 300., 0., 0., -90., 0.);
	ht   = gentr_pao("HT"  , 100., 0., 50., 0., 1., 0., 0., 0., -1.);
	pha  = gentr_eul("PHA" , 0., 0., -20., 0., 15., 0.);
	pch  = gentr_eul("PCH" , 0., 0., 0., 0., 15., 0.);
	pac  = gentr_eul("PAC" , 0., 0., 0., 0., 0., 0.);
	pn   = gentr_eul("PN"  , 0., 0., 40., 0., 0., 0.);

	pos1 = makeposition("P1", z, t6, e, EQ, p, pa, TL, e);
	pos2 = makeposition("P2", z, t6, e, EQ, p, pg, TL, e);
	pos3 = makeposition("P3", z, t6, e, EQ, p, pd, pg, TL, pg);
	pos4 = makeposition("P3", z, t6, e, EQ, h, ht, pha, pg, TL, pg);
	pos5 = makeposition("P5", z, t6, e, EQ, h, ht, pch, pg, TL, pg);
	pos6 = makeposition("P6", z, t6, e, EQ, h, ht, pac, pg, TL, pg);
	pos7 = makeposition("P7", z, t6, e, EQ, h, ht, pn, pg, TL, pg);
	pos8 = makeposition("P8", z, t6, e, EQ, h, ht, pn, pa, TL, e);

	for (; ; ) {
		movejnts(pos1, 200, 2000);
		movecart(pos2, 100, 700);
		stop(0);
		movecart(pos3, 100, 700);
		movejnts(pos4, 100, 500);
		movecart(pos5, 100, 500);
		movejnts(pos6, 100, 700);
		movecart(pos7, 100, 700);
		stop(0);
		movecart(pos8, 100, 700);
		printf("more ?");  QUERY(q);
		if (q == 'n')
			break;
	}
	movejnts(park, 200, 2000);
}



cvsj1()
{
	TRSF_PTR z, e , b1, s1, s2, s3, conv1, conv2;
	POS_PTR  p0;
	POS_PTR p1[4], p2[4];
	int convfn1();
	int i, j, q;

	conv1 = newtrans("CONV1",convfn1);
	conv2 = newtrans("CONV2",convfn1);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	b1 = gentr_rot("B1", 600. , -400., 500., yunit, 180.);
	s1 = gentr_trsl("S1", 0., 0., 80.);
	s2 = gentr_trsl("S2", 80., 0., 80.);
	s3 = gentr_trsl("S3", 80., 0., 0.);

	p0 = makeposition("P0" , z, t6, e, EQ, b1, TL, e);
	p1[0] = makeposition("P10" , z, t6, e, EQ, conv1, b1, TL, e);
	p1[1] = makeposition("P11" , z, t6, e, EQ, conv1, b1, s1, TL, e);
	p1[2] = makeposition("P12" , z, t6, e, EQ, conv1, b1, s2, TL, e);
	p1[3] = makeposition("P13" , z, t6, e, EQ, conv1, b1, s3, TL, e);
	p2[0] = makeposition("P20" , z, t6, e, EQ, conv2, b1, TL, e);
	p2[1] = makeposition("P21" , z, t6, e, EQ, conv2, b1, s1, TL, e);
	p2[2] = makeposition("P22" , z, t6, e, EQ, conv2, b1, s2, TL, e);
	p2[3] = makeposition("P23" , z, t6, e, EQ, conv2, b1, s3, TL, e);


	movejnts(p0, 100, 1500);
	for (; ; ) {
		waitfor(completed);
		rtime = 0;
		for (j = 0; j < 6; ++j) {
			for (i = 0; i < 4; ++i) {
				movecart(p1[i], 100, 300);
			}
		}
		movejnts(p0, 100, 1500);
		waitfor(completed);
		rtime = 0;
		for (j = 0; j < 6; ++j) {
			for (i = 0; i < 4; ++i) {
				movejnts(p2[i], 100, 300);
			}
		}
		movejnts(p0, 100, 1500);
		waitfor(completed);
		printf("more ? ");
		QUERY(q);
		if (q == 'n') break;
	}
	movejnts(park, 100, 1500);
}


convfn1(t)
TRSF_PTR t;
{
	t->p.y = .08 * rtime;
}



cvsj2()
{
	TRSF_PTR z, e, conv, or, fl;
	POS_PTR  pm, pf;
	int convfn2();
	int i;

	conv = newtrans("CONV",convfn2);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	or = gentr_eul("OR", 000. , 600., 500.,    0., 0., 90.);
	fl = gentr_rot("FL", 0. , 0., 0., yunit, 180.);

	pf = makeposition("PF" , z, t6, e, EQ, or, fl, TL, e);
	pm = makeposition("PM" , z, t6, e, EQ, conv, or, fl, TL, e);


	movejnts(pf, 300, 1500);
	for (; ; ) {
		printf("cart ");QUERY(i);
		if (i == 'y') {
			movecart(pm, 100, 800);
			for (i = 0; i < 6; ++i) {
				movecart(pm, 100, 800);
			}
		}
		movecart(pf, 100, 800);
		printf("jnts ");QUERY(i);
		if (i == 'y') {
			movejnts(pm, 100, 800);
			for (i = 0; i < 6; ++i) {
				movejnts(pm, 100, 800);
			}
		}
		movecart(pf, 100, 800);
		printf("quit ");QUERY(i);
		if (i == 'y') {
			break;
		}
		movecart(pf, 100, 800);
	}
	movecart(park, 300, 1500);
}



convfn2(t)
TRSF_PTR t;
{
	extern int rtime;
	double time;

	double omega = .220;
	double radius = 200.;

	time = rtime / 1000.;
	t->p.x = radius * cos(omega * time * PIT2);
	t->p.z = radius * sin(omega * time * PIT2);
}


spire()
{
	int spirefn();

	int q;

       TRSF_PTR
	base = gentr_rot("BASE", -200., 100., 200., yunit, 0.),
	tool = gentr_rot("TOOL", 0., 0., 180., yunit, 0.),
	pin = gentr_rot("PIN", 400. , 300., 0., zunit, 0.),
	pintip = gentr_rot("PINTIP", 0., 0., 50., yunit, 180.),
	spire = newtrans("SPIRE", spirefn);

	POS_PTR
	touch = makeposition("TOUCH",
		base, t6, tool, EQ, pin, pintip, TL, tool),
	spiral = makeposition("SPIRAL",
		base, t6, tool, EQ, spire, pin, pintip, TL, tool);

	setvel(200, 100);

	for (; ; ) {
		setmod('c');
		move(touch);
		QUERY(q); if (q == 'n') break;
		setime(200, 15000);
		move(spiral);
	}
	setmod('j');
	move(park);
}

/*
 * generate a spiraling motion in the x y plan
 */

spirefn(t)
TRSF_PTR t;
{
	real radius = goalpos->scal * 50.;
	real turns = 10.;
	real heigth = 100.;
	real angle = - 5.;
	VECT k;

	t->p.x = radius * (k.x = cos(turns * goalpos->scal * PIT2));
	t->p.y = radius * (k.y = sin(turns * goalpos->scal * PIT2));
	t->p.z = heigth * goalpos->scal;
	k.z = 0.;
	Rot(t, &k, angle * goalpos->scal);
}



psv()
{
	TRSF_PTR z, e , b1, conv;
	POS_PTR  p1;
	int convfo();
	int q;
	double iy;

	conv = newtrans("CONV",convfo);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	b1 = gentr_rot("B1", 600. , 0., 500., yunit, 180.);

	b1->fn = hold;
	p1 = makeposition("P1" , z, t6, e, EQ, conv, b1, TL, e);

	setmod('c');
	setvel(300, 100);
	move(p1);
	for (; ; ) {
		move(p1);
		waitfor(completed);
		if (p1->code == LIMIT) {
			printf("limit reached\n");
		}
		printf("more "); QUERY(q); if (q == 'n') break;
		printf("enter Z increment ");
		scanf("%f", &iy);
		b1->p.z += iy;
	}
	setmod('j');
	move(park);
}


convfo(t)
TRSF_PTR t;
{
	double time;

	time = rtime / 20000.;
	t->p.y = 400. * cos(time * PIT2);
}


int sensor;

#define TOUCHED 10

ftrk()
{
	TRSF_PTR z, k1, k2, finf, abov;
	TRSF_PTR b1, b2, b3, b4, b5, over, fing, getit, flip;
	POS_PTR  q1, q2, got;
	POS_PTR  p1, p2, p3, p4, p5, pp1, pp2, pp3, pp4, pp5, get;
	int touchfn();
	int fingfn();
	int q;

	finf = newtrans("FINK",fingfn);
	rot(finf, zunit, -90.);
	fing = gentr_rot("FINS", 0., 0., 0., zunit, -90.);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.);
	k1 = gentr_rot("K1", 600. ,-300., 450., yunit, 180.);
	k2 = gentr_rot("K2", 600. , 200., 450., yunit, 180.);
	abov = gentr_rot("OVER", 600., 0., 600., yunit, 180.);
	b1 = gentr_trsl("B1", 600. ,-200., 300.);
	b2 = gentr_trsl("B2", 600. ,-100., 300.);
	b3 = gentr_trsl("B3", 600. , 000., 300.);
	b4 = gentr_trsl("B4", 600. , 100., 300.);
	b5 = gentr_trsl("B5", 600. , 200., 300.);
	over = gentr_rot("OVER", 0., 0., 100., yunit, 180.);
	flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);
	getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);


	q1 = makeposition("Q1" , z, t6, fing, EQ, k1, TL, fing);
	q2 = makeposition("Q2" , z, t6, finf, EQ, k2, TL, finf);
	got = makeposition("GOT", z, t6, EQ, abov, TL, t6);
	p1 = makeposition("P1" , z, t6, fing, EQ, b1, over,TL , fing);
	p2 = makeposition("P2" , z, t6, fing, EQ, b2, over,TL , fing);
	p3 = makeposition("P3" , z, t6, fing, EQ, b3, over,TL , fing);
	p4 = makeposition("P4" , z, t6, fing, EQ, b4, over,TL , fing);
	p5 = makeposition("P5" , z, t6, fing, EQ, b5, over,TL , fing);
	pp1 = makeposition("PP1" , z, t6, fing, EQ, b1, flip, TL , fing);
	pp2 = makeposition("PP2" , z, t6, fing, EQ, b2, flip, TL , fing);
	pp3 = makeposition("PP3" , z, t6, fing, EQ, b3, flip, TL , fing);
	pp4 = makeposition("PP4" , z, t6, fing, EQ, b4, flip, TL , fing);
	pp5 = makeposition("PP5" , z, t6, fing, EQ, b5, flip, TL , fing);
	get = makeposition("GET", z, t6, EQ, getit, TL, t6);



	movejnts(get, 400, 1500);
	waitfor(completed);
	OPEN;
	printf("put the sensor in the jaws ");
	QUERY(q);
	CLOSE;
	printf("go ahead ");
	QUERY(q);
	if (q == 'n') {
		movejnts(park, 400, 2500);
		return;
	}
	sensor = adcopen(7);
	for (; ; ) {
		movecart(q1, 400, 1500);
		stop(0);
		sample(15);
		movecart(q2, 200, 8000);
		waitfor(completed);
		sample(30);
		QUERY(q); if (q == 'n') break;
		movecart(got, 400, 1500);
	}
	movejnts(get, 400, 1500);
	printf("type 'y' for poking");
	QUERY(q);
	if (q == 'n') {
		movejnts(park, 400, 2500);
		waitfor(completed);
		OPEN;
		return;
	}
	for (; ; ) {
		movejnts(p1, 60, 1500);
		evalfn(touchfn);
		movejnts(pp1, 60, 1500);
		movejnts(p2, 60, 1500);
		evalfn(touchfn);
		movejnts(pp2, 60, 1500);
		movejnts(p3, 60, 1500);
		evalfn(touchfn);
		movejnts(pp3, 60, 1500);
		movejnts(p4, 60, 1500);
		evalfn(touchfn);
		movejnts(pp4, 60, 1500);
		movejnts(p5, 60, 1500);
		evalfn(touchfn);
		movejnts(pp5, 60, 1500);
		movecart(get, 60, 1500);
		waitfor(pp1->end)
		if (pp1->code == TOUCHED) printf("touched\n");
		else printf("not touched\n");
		waitfor(pp2->end)
		if (pp2->code == TOUCHED) printf("touched\n");
		else printf("not touched\n");
		waitfor(pp3->end)
		if (pp3->code == TOUCHED) printf("touched\n");
		else printf("not touched\n");
		waitfor(pp4->end)
		if (pp4->code == TOUCHED) printf("touched\n");
		else printf("not touched\n");
		waitfor(pp5->end)
		if (pp5->code == TOUCHED) printf("touched\n");
		else printf("not touched\n");

		printf("more ? ");
		QUERY(q); if (q == 'n') break;
	}
	movejnts(park, 400, 1500);
	waitfor(completed);
	OPEN;
}


touchfn()
{
	if (how.adcr[sensor] > 1) {
		nextmove = TOUCHED;
	}
}


fingfn(t)
TRSF_PTR t;
{
	t->p.z += (how.adcr[sensor] * .01 - 3.) / 3.;
}



pcnf()
{
	TRSF_PTR z, e , b1, rt;
	POS_PTR  p1, p2;
	int q;

	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	e = gentr_eul("E" , 0. , 0. , 170. , 0. , 0.,  0.);/* finger tips */
	b1 = gentr_pao("B1", -128. , 800.,  900., 0., 0., -1., 0., 1., 0.);
	rt = rot(newtrans("RT", const), xunit, 90.);

	p1   = makeposition("P1" , z, t6, e, EQ, b1, TL, e);
	p2   = makeposition("P2" , z, t6, e, rt, EQ, b1, TL, e);


	movejnts(p1, 300, 2000);
	for (; ; ) {
		printf("the arm is lun, goes luf ? ");
		QUERY(q); if (q == 'n') break;
		moveconf(p2, 300, 1000, "f");
		printf("the arm is luf, goes ldn ? ");
		QUERY(q); if (q == 'n') break;
		moveconf(p2, 300, 2000, "dn");
		printf("the arm is ldn, goes lun ? ");
		QUERY(q); if (q == 'n') break;
		moveconf(p1, 300, 2000, "u");
	}
	moveconf(park, 300, 2000, "lun");
	printf("END\n");
}


