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

pumatask()
{
	TRSF_PTR z, e, peg, hole, roty, bottom, angle;
	POS_PTR align, in, touch;
	int q;

	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_trsl("E" , 0. , 0. , 140.);
	peg = gentr_trsl("PEG", 0., 0., 10.);
	hole = gentr_trsl("HOLE", -50., 450., 500.);
	hole->fn = varb;
	bottom = gentr_trsl("OVER", 0., 0., -20.);
	roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
	angle = gentr_rot("ANGLE", 0., 0., 0., yunit, 12.);

	align = makeposition(
	"ALIGN", z, t6, e, peg, EQ, hole, roty, TL, peg);

	touch = makeposition(
	"TOUCH", z, t6, e, peg, EQ, hole, angle, roty, TL, peg);

	in = makeposition(
	"IN", z, t6, e, peg, EQ, hole, bottom, roty, TL, peg);

	setvel(300, 50);
	move(align);
	if (!teach(hole, align)) {
		setvel(300, 50);

		distance("dz", -100.);
			move(align);

		setmod('j');
		move(park);
		return;
	}
	setmod('c');
	setvel(100, 100);

	distance("dz", -10.);
		move(touch);

	QUERY(q);

	if (q == 'n') {
		setvel(300, 100);
		setmod('j');
		move(park);
		return;
	}


	setvel(4, 7);
	distance("dz", -4.);
		move(touch);

	limit("fz", 25.);
	distance("dz", 5.);
		move(touch);

	comply("fz", 15.);
		move(align);
	lock("fz");

	comply("fx fy", 0., 0.);
		distance("dz", 5.);
			move(align);

		limit("fz", 20.);
		move(in);

		update(hole, in);
		limit("fz tx", 40., 10.);
		distance("rx", 2.);
			move(in);

		update(hole, in);
		limit("fz tx", 40., 10.);
		distance("rx", -2.);
			move(in);

		update(hole, in);
		limit("fz ty", 40., 10.);
		distance("ry", 2.);
			move(in);

		update(hole, in);
		limit("fz ty", 40., 10.);
		distance("ry", -2.);
			move(in);

		update(hole, in);
		limit("fz", 20.);
		distance("dz", 10.);
			move(in);

		limit("dx dy", 1., 1.);
		move(align);
	lock("fx fy");

	setvel(50, 50);
	distance("dz", -50.);
		move(align);

	setvel(300, 50);
	setmod('j');
	move(park);
}
