/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include <stdio.h>
#include "sys.h"
#include "../h/moper_com.h"

#define HOST_IOTIME	50	/* we allow HOST 50 ticks to respond*/

#define CBREAK 0x1		/* terminal IO control options */
#define RAW    0x2
#define NOECHO 0x4

int softError = 0;		/* soft error; try to send code back to host */
int hardError = 0;		/* hard error; don't send code back to host */
int hostNotifiedOfError = 0;	/* error code has been sent back to host */

int activeSet = 0;		/* control interaction was activated */
int idleSet = 0;		/* control interaction was terminated */

#define RELEASED	1	/* codes for 'idleSet'	*/
#define SOFTERROR	2	/*   ...		*/

/* Used to set up points to create a fake error conditions for testing. */

extern unsigned short maintCom;
#define FAKE_SOFTERROR(x)	if (maintCom == (x)) 		\
				 { softError = (x);		\
				   maintCom = 0;		\
				 }
#define FAKE_HARDERROR(x)	if (maintCom == (x)) 		\
				 { maintCom = 0;		\
				   harderror(x);		\
				 }

int opFlags = 0;		/* operating mode (master, slave, etc) */
int resetEnv[10];		/* longjmp environment for harderror() */

extern int hostError;	/* errors set by the host I/O package */
extern int hostLocate;	/* location code set by the host I/O package */
extern int rbtError;	/* errors set by the robot interface package */

/* A little package that allows us to wath the execution activity 
   of the robot control routines on a scope */

extern int traceBit;
extern int powerChan;
extern unsigned short *pioOutAddr[];
#define SET_TRACE()	(*pioOutAddr[powerChan] |= traceBit)
#define CLEAR_TRACE()	(*pioOutAddr[powerChan] &= ~traceBit)

/* A little package that allows to request and derequest watchdog 
   timeouts at various intervals (in units of the system clock */

int timoutEnable = 1;
#define SET_TIMEOUT(c,t)	if(timoutEnable) timeout(ontimeout,(c),(t));
#define CLEAR_TIMEOUT()		timeout(0);

#define WAKEUP_SIG	1	/* host signal for slave mode wakeup */
#define RELEASE_SIG	2	/* host signal used to indicate release */

extern unsigned short *hostAddr;

/*DEBUG*/
int foobit = 0;
int cycle = 0;
int rbit = 0;
long time;
#define OCSR	(*(short*)0177564)
#define OBUF	(*(char*)0177566)
#define PUTC(c)	while(!(OCSR&0x80)); OBUF = (c);       

char *getErrMsg();

int hardErCnt;			/* proof against infinite hard err loops */
harderror (n, loc)
int n, loc;
{
	/* hard error routine ... abort/restart on severe conditions */

	if (++hardErCnt >= 3)
	 { printf ("Fatal - triple hard error\n");
	   zexit (100);
	 }
	hardError = n;
	CLEAR_TIMEOUT();
	hostDetach ();

	printf ("harderror 0x%x: %s at %d\n", n, getErrMsg(n), loc);

	longjmp (resetEnv, 1);
}

extern long gettime();

ontimeout (n)
int n;
{
	/* watchdog timeout routine */

	long time2 = gettime();

	hardError = MPR_TIMEOUT_E;
	hostDetach ();

	printf ("timeout: location (%d,%d)\n", n, hostLocate);

	longjmp (resetEnv, 1);
}

extern startup();
extern onrelease();		
extern slaveDrive();
extern masterDrive();
extern idleDrive();
extern rbtExit();

#define IDLE_INTERVAL	28	/* idle interval time, in milliseconds */
int idleInterval = IDLE_INTERVAL;

extern int baseClkRate;

main()
{
	hardErCnt = 0;
	onexit (rbtExit, 0);
	paramInit();	/* set up basic parameters, address, etc. */
	rbtInit();	
	FAKE_HARDERROR(FHE_UCLGAINS);

	ioctl (0, CBREAK, 0);

	if (setjmp(resetEnv) != 0)
	 { rbtExit();
	 }
	else
	 { rbtExit();
	   printf ("Moper started\n\n");
	 }

	while (1)
	 { hardError = softError = 0;
	   idleSet = activeSet = 0;
	   hostNotifiedOfError = 0;

	   rbtSetup();

	   clrtime();
	   clkast (idleDrive, 0, (idleInterval*10)/baseClkRate);	      
   
	   printf ("listening\n");

	   hostListen (startup, 0);

	   hardErCnt = 0;
   
	   while (1)
	    { monitor();
	      if (activeSet)
	       { printf ("active (%s)\n",
			  (opFlags & MASTER_F) ? "master" : "slave");
		 activeSet = 0;
	       }
	      if (idleSet)
	       { if (idleSet == SOFTERROR)
		  { printf ("error 0x%x: %s\n", 
			     softError, getErrMsg(softError));
		  }
		 else
		  { printf ("released\n");
		  }
		 idleSet = 0;
		 break;
	       }
	    }
	 }
}

#define BUFSIZE	(MAX_DATABUF/sizeof(short))
unsigned short buffer[BUFSIZE];

#define RELSEDBG 	0	/* debug flag for release mechansim */

/* A little package to enbale us to catch and handle release signals,
   or temporarily block their delivery */

int relseBlk = 0;  
int relseReq = 0;
short relseEnv[10];
int relseCatch = 0;
char *relseStr;			/* debug string (used w.r.t. RELSEDBG) */

#define CATCH_RELEASE(s) \
	( (setjmp(relseEnv) != 0) || checkRelease(s))
#define FLOAT_RELEASE(s)	relseCatch = 0; relseStr = (s);
#define BLOCK_RELEASE()	relseBlk = 1;
#define UNBLK_RELEASE(s) \
	relseBlk = 0; if(relseReq){ relseReq = 0; relseStr=(s); dorelease();}

checkRelease(s)
char *s;
{
	relseStr = "CATCH";
	relseCatch = 1;
	if (idleSet)
	 { return (1);
	 }
	else
	 { relseStr = s;
	   return (0);
	 }
}

onrelease()
{
	/* main handler for the release signal from the host */

	if (relseBlk)
	 { relseReq = 1;
	 }
	else
	 { dorelease();
	 }
}

dorelease()
{
	int size;

	/* cancel all possible software interrupts */

	clkast (0);
	CLEAR_TIMEOUT();
	hostOnSig (WAKEUP_SIG, 0, 0);
	splx(0);

	if (!hostNotifiedOfError)
	 { /* read final release commands from host and send back a 
	      response */

rbit = 0;
	   SET_TIMEOUT (1, HOST_IOTIME);
rbit = 1;
	   if ((size = hostRead (buffer, BUFSIZE)) < 0)
	    { harderror (hostError, 26);
	    }
	   FAKE_HARDERROR(FHE_RELREAD);
rbit = 2;
	   if (!softError)
	    { if (rbtCommands (buffer, size) < 0)
	       { softError = rbtError;
	       }
	    }
	   FAKE_SOFTERROR(FSE_RELEASE);
rbit = 3;
	   if (softError)
	    { if (hostWrite (&softError, 1) < 0)
	       { harderror (hostError, 27);
	       }
	      rbtExit();
	    }
	   else
	    { if (hostWrite (buffer, 0) < 0)
	       { harderror (hostError, 28);
	       }
	       FAKE_HARDERROR(FHE_RELWRITE);
	    }
rbit = 4;
	   CLEAR_TIMEOUT();
rbit = 5;
	 }
	idleSet = (softError ? SOFTERROR : RELEASED );

#if RELSEDBG
	if (strlen (relseStr) > 0)
	 { printf ("%s\n", relseStr);
	 }
#endif
rbit = 0;
	if (relseCatch)
	 { longjmp (relseEnv, 1);
	 }
}

startup()
{
	int size;
		
	clkast (0);

	splx(0);

	hostOnSig (RELEASE_SIG, onrelease, 0);	/* set up release handler */

	if (CATCH_RELEASE("START C"))
	 { return (0);
	 }
	SET_TIMEOUT (2, HOST_IOTIME);
	if (hostAttach() < 0)
	 { harderror(hostError, 29);
	 }
	FAKE_HARDERROR(FHE_ATTACH);
	BLOCK_RELEASE();
	size = rbtStart (buffer);
	UNBLK_RELEASE("START 1");
	hostAddr[1] = 0x2222;
	if (hostWrite (buffer, size) < 0)
	 { harderror (hostError, 30);
	 }
	if ((size = hostRead (buffer, BUFSIZE)) < 0)
	 { harderror (hostError, 31);
	 }
	CLEAR_TIMEOUT();
	BLOCK_RELEASE();
	if (rbtIcton (buffer, size) < 0)
	 { softError = rbtError;
	 }
	FAKE_SOFTERROR(FSE_ICTON);
	UNBLK_RELEASE("START 2");

	spl7();
	activeSet = 1;
	if (opFlags & MASTER_F)
	 { clkast (masterDrive, 0, getInterval());
	 }
	else
	 { hostOnSig (WAKEUP_SIG, slaveDrive, 0);
	   SET_TIMEOUT (3, getMaxDelay());
	 }
	cycle = 0;
	FLOAT_RELEASE("START F");
}

slaveDrive()
{
	splx(0);

	SET_TRACE();			/* so we can watch on the scope */
	CLEAR_TIMEOUT();

	if (CATCH_RELEASE("SDRIVE"))
	 { CLEAR_TRACE();
	   return;
	 }
	if (doDrive() == 0)
	 { CLEAR_TRACE();
	   return;
	 }
	spl7();
	hostOnSig (WAKEUP_SIG, slaveDrive, 0);
	SET_TIMEOUT (4, getMaxDelay());
	CLEAR_TRACE();
	FLOAT_RELEASE("SDRIVE F");
	cycle++;
}

masterDrive()
{
	int interval = 0;

	SET_TRACE();
	splx(0);

	if (CATCH_RELEASE("MDRIVE"))
	 { CLEAR_TRACE();
	   return;
	 }
	do
	 { if (interval < 0 && (-interval) > getMaxDelay())
	    { printf ("timeout %d vs %d\n", interval, getMaxDelay());
	      softError = MPR_TIMEOUT_E;
	    }
	   interval = getInterval();
	   if (doDrive() == 0)
	    { CLEAR_TRACE();
	      return;
	    }
	 }
	while (interval <= 0);

	spl7();
	clkast (masterDrive, 0, interval);
	CLEAR_TRACE();
	FLOAT_RELEASE("MDRIVE F");
}

doDrive()
{
	int size;

	BLOCK_RELEASE();
	if (!softError)
	 { if ((size = rbtGather (buffer)) < 0)
	    { softError = rbtError;
	    }
	 }
	FAKE_HARDERROR(FHE_UCGATHER);
	FAKE_SOFTERROR(FSE_GATHER);
	UNBLK_RELEASE("DRIVE 1");

	hostSignal();

	if (softError)
	 { SET_TIMEOUT (5, HOST_IOTIME);
	   if (hostWrite (&softError, 1) < 0)
	    { harderror (hostError, 32);
	    }
	   CLEAR_TIMEOUT();
	   hostNotifiedOfError = 1;
	   rbtExit();
	   dorelease();
	 }
	else
	 { SET_TIMEOUT (6, HOST_IOTIME);

	   time = gettime();
	   if (hostWrite (buffer, size) < 0)
	    { harderror (hostError, 33);
	    }
	   FAKE_HARDERROR(FHE_WRITE);
	   if ((size = hostRead (buffer, BUFSIZE)) < 0)
	    { harderror (hostError, 34);
	    }
	   FAKE_HARDERROR(FHE_READ);
	   CLEAR_TIMEOUT();
	 }

	BLOCK_RELEASE();
	if (rbtCommands (buffer, size) < 0)
	 { softError = rbtError;
	 }
	FAKE_SOFTERROR(FSE_COMMAND);
	UNBLK_RELEASE("DRIVE 2");
	return (1);
}

/*DBG*/
int pulseData;
#define OUTBUF	(*(unsigned short*)0xfbfa)
#define CSR	(*(unsigned short*)0xfbf8)
idleDrive()
{
	SET_TRACE();

	rbtIdle();
	clkast (idleDrive, 0, (idleInterval*10)/baseClkRate);
	CLEAR_TRACE();
/*DBG*/
	if (pulseData)
	 { CSR = (CSR & 0x2) ? 0 : 2;
	 }
}

extern short jstat[];
extern short encread[];
extern short encset[];

#include "sys.h"

monitor()
{
	char cmd[2];

	if (TESTC())
	 { cmd[0] = getchar();		/* scanf ("%1s", cmd);	*/
	   putchar ('\n');
	   switch (cmd[0])
	    { case '?' :
	       { /*printf ("?\r\n");*/
		 printf (" '?': prints this message\n");
		 printf (" 'h': halts moper\n");
		 printf (" 't': enable or disable timeouts\n");
		 printf (" 'T': set up a timeout\n");
		 printf (" 'r': set rates\n");
		 printf (" 'g': get calibration data\n");
		 printf (" 'd': dump data\n");
		 printf (" 'p': memory peek routine\n");
		 printf (" 'P': memory poke routine\n");
		 printf (" 'R': restart moper\n");
		 printf (" 'm': get memory\n");
		 printf (" 'v': allocate vector\n");
		 printf (" 'o': octal to hex conversion\n");
		 printf (" 'x': hex to octal conversion\n");
		 printf (" 's': search for pattern\n");
/*DBG*/		 printf (" 'D': pulse output data buffer on\n");
/*DBG*/		 printf (" 'O': pulse output data buffer off\n");
		 break;
	       }
/*DBG*/
	      case 'D':
	       { pulseData = 1;
		 break;
	       }
	      case 'O':
	       { pulseData = 0;
		 break;
	       }
/*DBG*/
	      case 'T':
	       { int x;
		 printf ("time> ");
		 scanf ("%d", &x);
		 timeout (ontimeout, 123, x);
		 break;
	       }
	      case 'x':
	       { int x;
		 printf ("hex number> ");
		 scanf ("%x", &x);
		 printf ("%o\n", x);
		 break;
	       }
	      case 'o':
	       { int x;
		 printf ("octal number> ");
		 scanf ("%o", &x);
		 printf ("%x\n", x);
		 break;
	       }
	      case 'h':
	       { /*printf ("h\r\n");*/
		 zexit (0);
		 break;
	       }
	      case 't':
	       { printf ("timeout enable > ");
		 scanf ("%d", &timoutEnable);
		 if (timoutEnable)
		  { printf ("timeouts enabled\n");
		  }
		 else
		  { printf ("timeouts disabled\n");
		  }
		 break;
	       }
	      case 'r':
	       { int i, o;
		 printf ("interval and offset> ");
		 if (scanf ("%d%d", &i, &o) != 2)
		  { printf ("error reading values\n");
		    break;
		  }
		 if (setRate (i,o) < 0)
		  { printf ("error setting rates\n");
		  }
		 break;
	       }
	      case 'm':
	       { int size;

		 printf ("size> ");
		 scanf ("%d", &size);
		 printf ("getmem returns 0x%x\n", getmem(size));
		 break;
	       }
	      case 'v':
	       { VECT_DESC vd;
		 int (*func)();
		 unsigned short *vect;
		 int arg, save;

		 printf ("func, arg, vector, save> ");
		 scanf ("%x%x%x%x", &func, &arg, &vect, &save);
		 printf ("func = 0x%x arg = 0x%x vector = 0x%x save = 0x%x\n",
			func, arg, vect, save);
		 vd.fxn = func;
		 vd.arg = arg;
		 vd.saveregs = save;
		 vectorAlloc (vect, &vd);
		 break;
	       }
	      case 'g':
	       { extern short markFound;
		 extern short oldMarkFound;
		 extern short jntmode[];
		 int j;

		 printf ("JNT  jstat encread  encset\n");
/*			  XX  XXXXXX  XXXXXX  XXXXXX	*/
		 for (j=0; j<6; j++)
		  { printf ("%2d  %6x  %6d  %6d\n", j+1, jstat[j],
			     encread[j], encset[j]);
		  }
		 break;
	       }
		 
	      case 'd':
	       { unsigned *addr;
		 int i, num;

		 printf ("address(hex) and num words > ");
		 scanf ("%x%d", &addr, &num);
		
		 addr = (unsigned *)((int)addr & ~1);
		 for (i=0; i<num; i++)
		  { printf ("0x %4x: %4x\n", &addr[i], addr[i]);
		  }
		 break;
	       }
	      case 'p':
	       { unsigned short *addr;
		 printf ("address (hex)> ");
		 if (scanf ("%x", &addr) != 1)
		  { printf ("error reading number\n");
		  }
		 else
		  { addr = (unsigned short*)((int)addr & ~1);
		    printf ("%x = %x\n", addr, *addr);
		  }
		 break;
	       }
	      case 'P':
	       { unsigned short *addr;
		 unsigned short value;
		 printf ("address and value (hex)> ");
		 if (scanf ("%x%x", &addr, &value) != 2)
		  { printf ("error reading numbers\n");
		  }
		 else
		  { addr = (unsigned short*)((int)addr & ~1);
		    *addr = value;
		    printf ("%x = %x\n", addr, *addr);
		  }
		 break;
	       }
	      case 'R':
	       { longjmp (resetEnv, 0);
		 break;
	       }
	      case 's':
	       { unsigned short value;
		 unsigned short *start;
		 unsigned short *stop;

		 printf ("pattern, startAddress, stopAddress (hex)> ");
		 if (scanf ("%x%x%x", &value, &start, &stop) != 3)
		  { printf ("error reading values\n");
		    break;
		  }
		 /* word align addresses */
		 start = (unsigned short *)((int)start & ~0x1);
		 stop  = (unsigned short *)((int)stop & ~0x1);
		 for ( ; start <= stop; start++)
		  { if (*start == value)
		     { printf ("0x%x: %x\n", start, value);
		     }
		  }
		 break;
	       }
	    }
	 }
}

char *getErrMsg(code)
int code;
{
	switch (code)
	 { case MPR_BADRATE_E:
	    { return ("illegal rate request");
	      break;
	    }
	   case MPR_TIMEOUT_E:
	    { return ("timeout waiting for host");
	      break;
	    }
	   case MPR_MEMINIT_E:
	    { return ("error intializing memory");
	      break;
	    }
	   case MPR_ESTOP_E:
	    { return ("emergency stop set");
	      break;
	    }
	   case HST_TIMEOUT_E:
	    { return ("timeout talking to host");
	      break;
	    }
	   case HST_HEADER_E:
	    { return ("bad header from host");
	      break;
	    }
	   case HST_CHKSUM_E:
	    { return ("checksum error on host data");
	      break;
	    }
	   case HST_COUNT_E:
	    { return ("bad host packet count");
	      break;
	    }
	   case HST_SYNC_E:
	    { return ("lost sync with host");
	      break;
	    }
	   case HST_BADCMD_E:
	    { return ("bad command from host");
	      break;
	    }
	   case SRV_TIMEOUT_E:
	    { return ("joint servo timeout");
	      break;
	    }
	  case SLE_LIMIT_E:
	    { return ("hard joint limit");
	      break;
	    }
	  case SLE_POWERAMP_E:
	    { return ("power amp failure");
	      break;
	    }
	  case SLE_ENCODERS_E:
	    { return ("encoder failure");
	      break;
	    }
	  case SLE_BRAKES_E:
	    { return ("brakes release is enabled");
	      break;
	    }
	  default:
	    { if (code > 100)
	       { return ("error diagnostic");
	       }
	      else
	       { return ("???");
	       }
	    }
	 }
}
