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

extern struct how how;

static int sensor;

#define TOUCHED 10

pumatask()
{
	int touchfn();
	TRSF_PTR z, b1, b2, fing, getit, flip;
	POS_PTR  get, p1, p2;
	int q;

	z = gentr_trsl("Z",  0.,  0., 864.);
	b1 = gentr_trsl("B1", 600. ,-200., 400.);
	b2 = gentr_trsl("B2", 600. ,-100., 400.);
	fing = gentr_rot("FING", 0., 0., 200., zunit, -90.);
	getit = gentr_rot("GETIT", 600. , 0., 600., yunit, 180.);
	flip = gentr_rot("FLIP", 0., 0., 0., yunit, 180.);

	p1 = makeposition("P1" , z, t6, fing, EQ, b1, flip,TL , fing);
	p2 = makeposition("P2" , z, t6, fing, EQ, b2, flip,TL , fing);
	get = makeposition("GET", z, t6, EQ, getit, TL, t6);

	setvel(300, 100);
	move(get);
	waitfor(completed);
	OPEN;
	printf("put the sensor in the jaws ");
	QUERY(q);
	CLOSE;
	printf("go ahead ");
	QUERY(q);
	if (q == 'n') {
		move(park);
		return;
	}
	sensor = adcopen(7);
	setvel(100, 100);
	for (; ; ) {
		p1->end = p2->end = 0;
		move(p1);
		evalfn(touchfn);
		setime(0, 0);
		distance("dz", 100.);
		move(p1);
		move(p1);

		move(p2);
		evalfn(touchfn);
		setime(0, 0);
		distance("dz", 100.);
		move(p2);
		move(p2);

		waitfor(p1->end)
		printf("guarded motion 1 starts\n");
		waitfor(p1->end)
		if (p1->code == TOUCHED)
			printf("touched\n");
		else
			printf("not touched\n");

		waitfor(p2->end)
		printf("guarded motion 2 starts\n");
		waitfor(p2->end)
		if (p2->code == TOUCHED)
			printf("touched\n");
		else
			printf("not touched\n");

		printf("more ? ");
		QUERY(q); if (q == 'n') break;
	}
	setvel(300, 100);
	move(park);
	waitfor(completed);
	OPEN;
}


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