/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : main.c
 *      Remarks : From where everything begins, heavyly dependent on
 *                the version, implements what is not needed from the
 *                real time library. Opens and close files.
 *                The real time library functions are replaced
 *                by pseudo ones (manip.h _l) by macro process.
 *      Usage   : part of the library
 */

#include <ctype.h>
#include <signal.h>
#include "../h/which.h"
#include "../h/switch.h"
#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/kine.h"
#include "../h/bio.h"
#include "../h/umac.h"

#define RW      2

#ifdef FAKE
static int pid, clockid;
#endif


main(argc, argv)
int argc;
char **argv;
{
	char st[80];
	char *s, *stp, *strcat();
	int q;
	int fddbo;
	bool prints_save;
	char *mduper = "can't duplicate data base file\n";
	char bd[BUFS];

	while(--argc > 0 && **++argv == '-') {
		for(s = *argv+1; *s; s++) { /* scan multiple arg */
			switch (*s) {
			case 'v' : /* verbose */
				prints_out++;
				break;

			case 'D' : /* data base */
				++s;
				for (stp = st; *s != '\0'; ) {
					*stp++ = *s++;
				}
				*stp = '\0';
				if ((fddb = open(st, RW)) < 0) {
					printf("%s does'nt exit, create ?", st);
					QUERY(q);
					if (q == 'y') {
						if ((fddb = makedb(st)) < 0) {
							exit(14);
						}
						if ((fddb = open(st, RW)) < 0) {
							exit(15);
						}
					}
				}
				else {
				 (void) strcat(st, ".old");
					if ((fddbo = creat(st, 0644)) < 0) {
						printf(mduper);
						exit(16);
					}
					while ((q = read(fddb, bd, BUFS)) > 0) {
						if (write(fddbo, bd, q) < 0) {
							printf(mduper);
							exit(17);
						}
					}
				}
				--s;
				break;

			case 'b' : /* no force stuff */
				force_ctl = NO;
				break;


#ifndef REAL    /* permit i/o s */
			case 'g' : /* graphics */
				opsw_n.graphics = YES;
				break;

			case 'd' : /* output relative solution angles */
				opsw_n.numerics = YES;
				break;

			case 'a' : /* output solution angles */
				opsw_n.angles = YES;
				break;

			case 'k' : /* output t6 but not j6 */
				opsw_n.t6butnotj6 = YES;
				break;

			case 'e' : /* output encoder values */
				opsw_n.encoders = YES;
				break;

			default :
				printf(" options -v -g -d -a -k -e -b -D*\n");
				exit(33);
#else
			default :
				printf(" options -v -b -D*\n");
				exit(33);
#endif
			}
		}
	}
	if (prints_out) {
		if ((fpi = fdopen(creat("@@.out", 0644), "w")) == NULL) {
			printf("can't create info file\n");
			exit(22);
		}
	}

	prints_save = prints_out;
	prints_out = NO;

	calibrate();
#ifdef PLAN
	startup();
	movejnts(park, 100, 300);
#else
	movejnts(park, 100, 300);
	startup();
#endif
	prints_out = prints_save;
#ifdef PUMA
	pumatask();
#endif
#ifdef STAN
	stantask();
#endif
	stop(300);
	waitfor(completed);
	release("task ended");
}

/*
 * according to the cases perform initial actions
 */

startup() /*::*/
{
	control(checkstate_n, setpoint_n);
#ifdef FAKE
	if ((clockid = vfork()) < 0) {
		fprintf(stderr, "can't fork\n");
		exit(13);
	}
	if (clockid == 0) {
		execl("clock");
		fprintf(stderr, "cant'n exec clock\n");
		exit(13);
	}
	pid = getpid();
#endif
}

/*
 * put process to sleep for 1/10 of a second
 */

suspendfg() /*::*/
{
#ifdef PLAN
	checkstate_n();
	setpoint_n();
#else
	nap(6);
#endif
}


/*
 * software error routine
 */
#ifndef REAL
/*ARGSUSED*/
#endif

giveup(m, user)  /*::*/
char *m;
bool user;
{
	terminate = YES;
	mess = m;
#ifndef REAL
	release("giveup");
	exit(1);
#else
	if (user) {
		release("giveup");
		mess = "";
		for (; ; ) {
			pause();
		}
		/*NOTREACHED*/
	}
#endif
}


#ifndef REAL
/*
 * replace rtc stuff by internal functions
 */

release(s) /*:: quit interrupt routine */
char *s;
{
#ifdef FAKE
 (void) signal(SIGCLCK, SIG_IGN);
 (void) kill(clockid, SIGKILL);
#endif
	if (opsw_n.encoders) {
		Close(&iobf_n[EB]);
	}
	if (opsw_n.graphics) {
		Close(&iobf_n[J1]);
		Close(&iobf_n[J2]);
		Close(&iobf_n[J3]);
		Close(&iobf_n[J4]);
		Close(&iobf_n[J5]);
		Close(&iobf_n[J6]);
		if (opsw_n.t6butnotj6) {
			Close(&iobf_n[6]);
			Close(&iobf_n[7]);
			Close(&iobf_n[8]);
			Close(&iobf_n[9]);
			Close(&iobf_n[10]);
			Close(&iobf_n[11]);
		}
		Close(&iobf_n[TB]);
		Close(&iobf_n[CB]);
	}
	if (opsw_n.numerics) {
	 (void) fclose(fpo);
	}
	if (prints_out) {
	 (void) fclose(fpi);
	}
	if ((int)s == SIGINT) {
		printf("time %d : Interrupted\n", rtime);
		exit(1);
	}
	else {
		printf("%s\n", mess);
		printf("pos %s time %d : %s\n", goalpos->name, rtime, s);
	}
	exit(0);
}


/*
 * Flush a buffer
 */

static Close(b) /*##*/
BIO_PTR b;
{
	int n;

	if (b->ptr != NULL) {
		n = b->ptr - b->buf;
		if (write(b->fd, b->buf, n) != n){
			fprintf(stderr, "write io error\n");
			exit(2);
		}
	}
	if (close(b->fd) < 0) {
		fprintf(stderr, "close io error\n");
		exit(2);
	}
}


#ifdef FAKE
static int (* fne1)();
static int (* fne2)();


static dummy() /*##*/
{
 (void) signal(SIGCLCK, dummy);
}

static onintr() /*##*/
{
 (void) signal(SIGCLCK, dummy);
	(* fne1)();
	(* fne2)();
 (void) signal(SIGCLCK, onintr);
}
#endif

#ifdef PLAN
/*ARGSUSED*/
#endif

control(fn1, fn2) /*::*/
int (* fn1)(), (* fn2)();
{

	if (opsw_n.numerics)  {
		if ((fpo = fdopen(creat("@.out", 0644), "w")) == NULL) {
			printf("can't open data file\n");
			exit(22);
		}
	}

	if (opsw_n.graphics) {
		iobf_n[J1].fd = creat("../g/f1.out", 0644);
		iobf_n[J2].fd = creat("../g/f2.out", 0644);
		iobf_n[J3].fd = creat("../g/f3.out", 0644);
		iobf_n[J4].fd = creat("../g/f4.out", 0644);
		iobf_n[J5].fd = creat("../g/f5.out", 0644);
		iobf_n[J6].fd = creat("../g/f6.out", 0644);
		if (opsw_n.t6butnotj6) {
			iobf_n[6].fd = creat("../g/f7.out", 0644);
			iobf_n[7].fd = creat("../g/f8.out", 0644);
			iobf_n[8].fd = creat("../g/f9.out", 0644);
			iobf_n[9].fd = creat("../g/f10.out", 0644);
			iobf_n[10].fd = creat("../g/f11.out", 0644);
			iobf_n[11].fd = creat("../g/f12.out", 0644);
		}
		iobf_n[TB].fd = creat("../g/t.out", 0644);
		iobf_n[CB].fd = creat("../g/c.out", 0644);
	}
	if (iobf_n[J1].fd < 0 || iobf_n[J2].fd < 0 || iobf_n[J3].fd < 0 ||
	    iobf_n[J4].fd < 0 || iobf_n[J5].fd < 0 || iobf_n[J6].fd < 0 ||
	    iobf_n[6].fd < 0 || iobf_n[7].fd < 0 || iobf_n[8].fd < 0 ||
	    iobf_n[9].fd < 0 || iobf_n[10].fd < 0 || iobf_n[11].fd < 0 ||
	    iobf_n[TB].fd < 0 || iobf_n[CB].fd < 0) {
		printf("can't create graphic files\n");
		exit(22);
	}
	if (opsw_n.encoders) {
		if ((iobf_n[EB].fd = creat("@@@.out",0644)) < 0) {
			printf("can't create encoder file\n");
			exit(22);
		}
	}
 (void) signal(SIGINT, release);
#ifdef FAKE
	fne1 = fn1;
	fne2 = fn2;
 (void) signal(SIGCLCK, onintr);
#endif
}
#endif



static calibrate() /*##*/
{
	assignjs_n(j6, &jcal_c);
	solveconf_n(j6);
	jns_to_tr_n(rest, j6, YES);
	park = makeposition("PARK", t6, EQ, rest, TL, t6);
	lastpos = goalpos = park;
	Assigntr(here, rest);
	there = makeposition("THERE", t6, EQ, here, TL, t6);
}
