#include "../h/rccl.h"
#include "../h/hand.h"
#include "../h/which.h"
#include "../h/kine.h"

pumatask()
{
	TRSF_PTR  z, e, cam, o, coord, t6r, u5i, expect, drop;
	POS_PTR look, get, put;

	z = gentr_trsl("Z",  0.,  0., 864.);
	e = gentr_trsl("E" , 0. , 0. , 170.);
	cam = gentr_rot("CAM", 0., 0., 50., xunit, 90.);
	expect = gentr_rot("EXPECT", 500. , 100., 600., yunit, 180.);
	drop = gentr_rot("DROP", 400. , -100., 500., yunit, 180.);
	o = newtrans("O", hold);
	coord = newtrans("COORD", hold);

	u5i = newtrans("U5I", varb);
	t6r = newtrans("T6R", varb);

	look = makeposition("LOOK", z, t6, e, EQ, expect, TL, e);
	get = makeposition("GET", t6, e, EQ, coord, cam, o, TL, e);
	put = makeposition("PUT" , z, t6, e, EQ, drop, TL, e);


	setvel(200, 100);
	for (; ; ) {
		move(look);
		waitas(goalpos == look && look->scal > .8);
		snapshot();
		Assigntr(t6r, t6);
		Invert(u5i, &sncs_d.u5);
		Trmult(coord, t6r, u5i);
		if (!getobj(o)) {
			break;
		}
		get->end = 0;
		distance("dz", -30.);
			move(get);
		move(get);
		stop(50);
		distance("dz", -30.);
			move(get);

		waitfor(get->end);
		waitfor(get->end);
		CLOSE;
		printf("closing\n");
		move(put);
		waitfor(put->end);
		OPEN;
		printf("openning\n");
	}
	move(park);
}

snapshot()
{
	printf("snap\n");
}

getobj(t)
TRSF_PTR t;
{
	static int number = 5;

	Trsl(t, 0., 0., 200. + number * 30.);
	Rot(t, yunit, 10. * number);
	return(number--);
}
