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

#define AWAYZ(p, l)      {distance("dz", - (l)); move(p);}
#define OVERSHOOTZ(p, l) {distance("dz",   (l)); move(p);}
#define FAST             setvel(300, 300.);
#define SLOW             setvel(50, 50.);
#define CAUTIOUS         setvel(7, 7);

/*
 * do one insertion
 */

insert(z, grip, peg, hole, depth, ang)
TRSF_PTR z, grip, peg, hole;
real depth, ang;
{
	TRSF_PTR bottom, angle, roty;
	POS_PTR align, in, touch;

	bottom = gentr_trsl("BOTTOM", 0., 0., -depth);
	angle = gentr_rot("ANGLE", 0., 0., 0., yunit, ang);
	roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);

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

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

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

	setmod('c');
	FAST
	AWAYZ(touch, 10.)
	CAUTIOUS
	AWAYZ(touch, 4.)
	limit("fz", 25.);
	OVERSHOOTZ(touch, 5.)
	comply("fz", 15.);
		move(align);
	lock("fz");
	comply("fx fy", 0., 0.);
		update(hole, in);
		limit("fz", 20.);
		OVERSHOOTZ(in, 10.);
	lock("fx fy");

	SLOW
	AWAYZ(align, 50.)
	waitfor(in->end)
	OPEN
	move(there);
	waitfor(completed);
	freepos(align);
	freepos(in);
	freepos(touch);
	freetrans(bottom);
	freetrans(angle);
	freetrans(roty);
	return;
}

/*
 * monitors feeder
 */

#define PARTS   1
#define EMPTY   2

monfeeder()
{
	if (feedersensor == PARTS) {
		nextmove = YES;
		CLOSE
	}
	if (feedersensor == EMPTY) {
		parts = 0;
	}
}

/*
 * Do insertions
 */

int parts = YES;

pumatask()
{
	TRSF_PTR z, e, assy, h, feeder, grasp, pegs;
	POS_PTR get;

	z = gentr();                            /* base frame */
	e = gentr();                            /* end effector */
	assy = gentr();                         /* assembly */
	grasp = gentr();                        /* gasp pos */
	feeder = gentr();                       /* feeder */
	pegs = gentr();                         /* peg rel. e */
	h = newtrans("H", hold);                /* h rel. to assy */

	get = makeposition("GET", z, t6, e, EQ, feeder, grasp, TL, e);

	while(parts) {
		move(get);
		evalfn(monfeeder);
		setime(200, 10000);
		move(get);
		gettr(h, file);
		insert(z, e, pegs, h, 20., 15.);
	}
}
