#include "../h/rccl.h"
#include "../h/rtc.h"
#include "../h/which.h"
#include "../h/kine.h"
#include "../h/umac.h"

extern struct how how;
extern struct chg chg;

int idx = 0;
char outc;

#define MAX     1000

double cur1[MAX];
double cur2[MAX];
double cur3[MAX];
double cur4[MAX];
double cur5[MAX];
double cur6[MAX];
double tor1[MAX];
double tor2[MAX];
double tor3[MAX];
double tor4[MAX];
double tor5[MAX];
double tor6[MAX];
char   cccc[MAX];

pumatask()
{
	TRSF_PTR z, e, beg, end;
	POS_PTR  p1, p2;
	int i;
	int fc1, fc2, fc3, fc4, fc5, fc6;
	int ft1, ft2, ft3, ft4, ft5, ft6;
	int fcc;
	int record();

	e = trsl(newtrans("E", const), 0., 0., 170.);
	z = gentr_rot("Z",  0.,  0., 864., zunit, 0.); /* at the base */
	beg = newtrans("BEG", varb);
	end = newtrans("END", varb);


	p1 = makeposition("P2", z, t6, e, EQ ,beg, TL, e);
	p2 = makeposition("P3", z, t6, e, EQ, end, TL, e);



	setvel(700, 700);
	setmod('j');
	if (!teach(beg,  p1)) {
		setvel(700, 700);
		setmod('j');
		move(park);
		return;
	}
	if (!teach(end,  p2)) {
		setvel(700, 700);
		setmod('j');
		move(park);
		return;
	}
	setmod('c');
	move(p2);

	setvel(70, 20);

	outc = '-';
	evalfn(record);
	move(p1);
	waitfor(completed);

	outc = '+';
	evalfn(record);
	move(p2);
	waitfor(completed);

	setvel(700, 700);
	move(park);
	stop(100);
	waitfor(completed);
	release("happy");
	fcc = creat("../g/c.out", 0644);
	write(fcc, cccc, idx);
	close(fcc);
	ft1 = creat("../g/tor1.out", 0644);
	ft2 = creat("../g/tor2.out", 0644);
	ft3 = creat("../g/tor3.out", 0644);
	ft4 = creat("../g/tor4.out", 0644);
	ft5 = creat("../g/tor5.out", 0644);
	ft6 = creat("../g/tor6.out", 0644);
	write(ft1, tor1, idx * sizeof(double));
	write(ft2, tor2, idx * sizeof(double));
	write(ft3, tor3, idx * sizeof(double));
	write(ft4, tor4, idx * sizeof(double));
	write(ft5, tor5, idx * sizeof(double));
	write(ft6, tor6, idx * sizeof(double));
	close(ft1);
	close(ft2);
	close(ft3);
	close(ft4);
	close(ft5);
	close(ft6);
	fc1 = creat("../g/cur1.out", 0644);
	fc2 = creat("../g/cur2.out", 0644);
	fc3 = creat("../g/cur3.out", 0644);
	fc4 = creat("../g/cur4.out", 0644);
	fc5 = creat("../g/cur5.out", 0644);
	fc6 = creat("../g/cur6.out", 0644);
	write(fc1, cur1, idx * sizeof(double));
	write(fc2, cur2, idx * sizeof(double));
	write(fc3, cur3, idx * sizeof(double));
	write(fc4, cur4, idx * sizeof(double));
	write(fc5, cur5, idx * sizeof(double));
	write(fc6, cur6, idx * sizeof(double));
	exit(0);
}





record()
{
	extern struct how how;
	static unsigned short old[6];
	double tor[6];
	JNTS lod;

	int i;

	if (goalpos->scal < .2 || goalpos->scal > .9) {
		for (i = 0; i < 6; ++i) {
			old[i] = how.pos[i];
		}
		return;
	}

	if (idx >= MAX) {
		return;
	}
	cur1[idx] = how.adcr[0];
	cur2[idx] = how.adcr[1];
	cur3[idx] = how.adcr[2];
	cur4[idx] = how.adcr[3];
	cur5[idx] = how.adcr[4];
	cur6[idx] = how.adcr[5];
	adctotor(tor, how.adcr, how.pos, old);
	gravload(&lod,
	sncs.c2, sncs.c23, sncs.s23, sncs.c4, sncs.s4, sncs.c5, sncs.s5);

	tor1[idx] = tor[0] - lod.th1;
	tor2[idx] = tor[1] - lod.th2;
	tor3[idx] = tor[2] - lod.th3;
	tor4[idx] = tor[3] - lod.th4;
	tor5[idx] = tor[4] - lod.th5;
	tor6[idx] = tor[5] - lod.th6;
	cccc[idx] = outc;

	++idx;

	for (i = 0; i < 6; ++i) {
		old[i] = how.pos[i];
	}
}
