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

pumatask()
{
	TRSF_PTR z, e , b1, b2, b3, b4, b5;
	POS_PTR  p1, p2, p3, p4, p5;
	int q;

	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_trsl("E" , 0. , 0. , 170.);
	b1 = gentr_rot("B1", 600. ,-100.,  300., yunit, 180.);
	b2 = gentr_rot("B2", 600. , 200.,  300., yunit, 180.);
	b3 = gentr_rot("B3", 600. , 200.,  400., yunit, 180.);
	b4 = gentr_rot("B4", 600. ,-100.,  400., yunit, 180.);
	b5 = gentr_rot("B5", 500. ,-100.,  300., yunit, 180.);

	p1   = makeposition("P1" , z, t6, e, EQ, b1, TL, e);
	p2   = makeposition("P2" , z, t6, e, EQ, b2, TL, e);
	p3   = makeposition("P3" , z, t6, e, EQ, b3, TL, e);
	p4   = makeposition("P4" , z, t6, e, EQ, b4, TL, e);
	p5   = makeposition("P5" , z, t6, e, EQ, b5, TL, e);


	setmod('c');
	setvel(200, 100);
	move(p4);
	for (; ; ) {
		QUERY(q); if (q == 'n') break;

		p1->end = 0;

		setvel(20, 20);
		limit("fz", 20.);
		move(p1);

		setvel(100, 50);
		comply("fz", 10.);
			move(p2);
		lock("fz");


		waitfor(p1->end);
		if (p1->code != ONF) {
			nextmove = YES;
			printf("where is the table ?\n");
			setvel(200, 100);
			move (park);
			return;
		}

		move(p3);
		move(p4);

		limit("fz", 20.);
		setvel(50, 50);
		move(p1);

		limit("dz", 3.);
		comply("fz", 10.);
			move(p5);
		lock("fz");

		move(p4);

		waitfor(p5->end);
		if (p5->code != OND) {
			printf("where is the edge ?\n");
		}
	}
	setvel(300, 50);
	move(park);
}
