/*-=-=-=-=-=-=-=-=-=-=-=- 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 "../h/moper_com.h"
#include "cdefs.h"
#include "pumaInterface.h"

#define PUMAJ	6
#define VERBOSE	0	/* debug prints; causes timeouts at fast rates */

#define CAL_LED		0x0001
#define NULL_LED	0x000e
#define M_NULL		0x8000
#define M_PROG_RUNNING  0x8d00

unsigned short integration = 0;
unsigned short calibMode = 0;

unsigned short markFound = 0;
unsigned short oldMarkFound = 0;
short calibrated = 0;

int torqueRead = 0;
int potRead = 0;
int pendantRead = 0;
int jstickRead = 0;
int forceRead = 0;
int testRead = 0;

unsigned short maintCom;

char adcOpen[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
char sluOpen[2] = { 0, 0 };

extern int numSlu;
extern int numPioIn;
extern int numPioOut;
extern int numDAC;
extern int numADC;

extern int moperID;
extern int robotID;

extern int torqueAvail;
extern int forceAvail;
extern int jstickAvail;
extern int potsAvail;
extern int pendantAvail;
extern int pendantMsgRun;

extern unsigned short getPioIn();
extern unsigned short getPioOut();

union _pack {
  unsigned char b[2];
  unsigned short s;
  };

extern int opFlags;
extern int maxRunReqs;

int rbtError;
unsigned short jntmode[PUMAJ];

unsigned short jstat[PUMAJ];	/* for debugging */
unsigned short encread[PUMAJ];
unsigned short ereadOk = 0;
unsigned short encset[PUMAJ];	

short clockRate = 0;	/* instantiated by clockSetup() */
short jointRate = 0;	/* instantiated by getJointRate() */

extern int powerChan;

static int log2();

rbtInit()
{
	/* called by moper when it is first started up (loaded) */

	extern unsigned short defaultRamTable[][];
	unsigned short value;
	int j;

	clockRate = clockSetup();
	jointRate = getJointRate();

	if (joint_ram_load (defaultRamTable) < 0)
	 { printf ("ERROR initializing RAM memory\n");
	   zexit (MPR_MEMINIT_E);
	 }

	for (j=0; j<PUMAJ; j++)
	 { value = (ereadOk ? encread[j] : 0100000);
	   if (putj (STOPM, value, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 1);
	    }
	   if (putj (POSET, value, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 1);
	    }
	   encset[j] = value;
	 }
	calibrated = (calibrated && ereadOk);
	ereadOk = 0;

	initJstick();
	initForce();
	initPendant();

	torqueRead = 0;
	potRead = 0;
	pendantRead = 0;
	jstickRead = 0;
	forceRead = 0;
	testRead = 0;

	if (powerChan == -1)
	 { powerOn();		/* Enable ARM power on only */
	 }
}

rbtSetup()
{
	/* called by moper before every new control cycle session */

	int j, k;

	for (j=0; j<PUMAJ; j++)
	 { unsigned short stat;
	   unsigned short value;
	   
	   if (getj (JSTAT, &stat, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 2);
	    }
	   if (stat & JFNDIXS)
	    { if (getj (READ, &value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 3);
	       }
	      encread[j] = value;
	      if (putj (STOPM, value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 4);
	       }
	      encset[j] = value;
	    }
	   if (putmode (MESERVO, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 5);
	    }
	   jntmode[j] = MESERVO;
	   integration = 0;
	   calibMode = 0;
	   markFound = 0;
	   oldMarkFound = 0;
	 }
	ereadOk = 1;

	setPendant((calibrated ? NULL_LED : CAL_LED) | M_NULL);
	if (!pendantRead)
	 { startPendant();
	   pendantRead = 1;
	 }
	jstickRead = 0;
	potRead = 0;
	stopJstick();
	forceRead = 0;
	torqueRead = 0;
	testRead = 0;
	stopForce();

	for (k=0; k<12; k++)
	 { adcOpen[k] = 0;
	 }
}

rbtStart(buffer)
unsigned short *buffer;
{
	unsigned short *dp = buffer;
	union _pack pack;
	int j;
/* KILROY */
	   int code1 = 0;
	   int code2 = 0;
/* WAS HERE */

	/* called by the host interrupt to start a control session */

	*dp++ = 0x5555;
	*dp++ = 0xaaaa;
	*dp++ = robotID;	
	*dp++ = moperID;
	*dp = 0;
	*dp |= torqueAvail ? 	0x01 : 0;
	*dp |= potsAvail ? 	0x02 : 0;
	*dp |= pendantAvail ? 	0x04 : 0;
	*dp |= forceAvail ? 	0x08 : 0;
	*dp |= jstickAvail ? 	0x10 : 0;
	*dp++;

	pack.b[0] = numADC;
	pack.b[1] = numDAC;
	*dp++ = pack.s;
	pack.b[0] = numPioIn;
	pack.b[1] = numPioOut;
	*dp++ = pack.s;
	pack.b[0] = numSlu;
	pack.b[1] = integration;
	*dp++ = pack.s;
	pack.b[0] = calibMode;
	pack.b[1] = markFound;
	*dp++ = pack.s;

#if READ_ENCSET
	for (j=0; j<6; j++)
	 { unsigned short value;
	   short diff;

	   if (getj (READ, &value, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 6);
	    }
/* KILROY */
	   if (value == 0x8000)
	    { code1 |= (1<<j);
	      if (getj (READ, &value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 6);
	       }
	      if (value == 0x8000)
	       { code2 |= (1<<j);
	       }
	    }
/* WAS HERE */
	   diff = value - encset[j];
	   if (ABS(diff) > 500)
	    { if (putj (POSET, value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 7);
	       }
	      encset[j] = value;
	    }
	   *dp++ = encset[j];
	 }
	if (code1)
	 { printf ("!%02x%02x\n", code1, code2);
	 }
#endif /*READ_ENCSET*/
	return (dp - buffer);
}

rbtIcton(buffer, size)
unsigned short *buffer;
int size;
{
	register unsigned short *cmdp = buffer;
	unsigned short *top = (unsigned short *)&buffer[size];
	register short com;

	int interval = -1;
	int offset = -1;
       
	/* go through the entire command buffer, taking out each command
	   and acting on it. Values associated with the command buffer
	   are removed if necessary */

	maxRunReqs = -1;
	opFlags = -1;

	while (cmdp < top)
	 { com = *cmdp++;

	   switch (com)
	    { case SET_RATE_C:
	       { interval = *cmdp++;
		 offset = *cmdp++;
		 break;
	       }
	      case SET_RUNREQ_C:
	       { maxRunReqs = *cmdp++;
		 break;
	       }
	      case START_C:
	       { opFlags = *cmdp++;
		 break;
	       }
	      case END_C:
	       { 
		 break;
	       }
	      case OFF_POWER_C:
	       { break;
	       }
	      default:
	       { harderror (HST_BADCMD_E, 8);
	       }
	    }
	 }
	if (interval < 0 || offset < 0 || opFlags < 0 || maxRunReqs < 0)
	 { harderror (HST_BADCMD_E, 9);
	 }
	if (setRate (interval, offset) < 0)
	 { setBaseRate ();
	   rbtError = MPR_BADRATE_E;
	   return (-1);
	 }
	setRobotRate(interval);

	/* XXX a bit of a hack here -- for some reason, the calib
	   light on the old teach pendant stays on if the LED field
	   is set to zero. So if it _is_ set to zero, we change the
	   setting to NULL_LED to turn the thing off. 
	 */
	if (pendantMsgRun & 0xff)
	 { setPendant ((calibrated ? 0 : CAL_LED) | pendantMsgRun);
	 }
	else
	 { setPendant ((calibrated ? NULL_LED : CAL_LED) | pendantMsgRun);
	 }
	return (1);
}

rbtGather (buffer)
unsigned short *buffer;
{
	unsigned short *dp = buffer;
	unsigned short *readFlags;
	union _pack pack;
	register short j;
	unsigned short state;

	if (rbtError = armError())
	 { return (-1);
	 }

	for (j=0; j<PUMAJ; j++)
	 { if (getj (JSTAT, &jstat[j], j) < 0)
	    { harderror (SRV_TIMEOUT_E, 10);
	    }
	 }

	servicePendant();
	for (j = 0; j<PUMAJ; j++)
	 { 
	   if (getj (READ, dp, j) < 0)
	    { harderror (SRV_TIMEOUT_E, 11);
	    }
	   encread[j] = *dp;
	   dp++;
	 }
	ereadOk = 1;
	*dp++ = getHndPos();
	*dp++ = getHndForce();

	state = 0;
	if (armPowerOn())
	 { state |= POWER_ON;
	 }
	if (calibrated)
	 { state |= IS_CALIBRATED;
	 }
	*dp++ = state;

	readFlags = dp++;
	*readFlags = 0;

	for (j=0; j<numPioIn; j++)
	 { *dp++ = getPioIn (j);
	 }
	for (j=0; j<numPioOut; j++)
	 { *dp++ = getPioOut (j);
	 }
	for (j=0; j<PUMAJ; j++)
	 { if (jntmode[j] & MLOOKZI && !(markFound & (1<<j)))
	    { unsigned short stat;
	      if (getj (JSTAT, &stat, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 12);
	       }
	      if (stat & JFNDIXS)
	       { markFound |= (1<<j);
		 putchar ('1'+j);
	       }
	    }
	 }
	if (markFound != oldMarkFound)
	 { *readFlags |= MARK_FOUND;
	   *dp++ = markFound;
	   oldMarkFound = markFound;
	 }
	if (torqueRead)
	 { getTorque (dp);
	   dp += 6;
	   *readFlags |= TORQUE_OK;
	 }
	if (potRead)
	 { extern int numPotBits;

	   for (j=0; j<PUMAJ; j++)
	    { extern int potRdCnt;
	      int k;

	      /* potRdCnt is a hack, put in because the old Mark I 
		 controller (MARKIO) can't seem to MUX its pot ADC fast
		 enough. We have to read each pot twice to give the ADC
		 time to settle on the correct value.
	       */
	      for (k=0; k<potRdCnt; k++)
	       { if (getj (READP, dp, j) < 0)
		  { harderror (SRV_TIMEOUT_E, 13);
		  }
	       }
	      *dp++ &= ((1<<numPotBits)-1);
	    }
	   *readFlags |= POT_OK;
	 }
	if (pendantRead && getPendant (dp))
	 { *readFlags |= PENDNT_OK;
	   dp += 3;
	 }
	if (forceRead && getForce (dp))
	 { *readFlags |= FORCE_OK;
	   dp += 6;
	 }
	if (jstickRead && getJstick (dp))
	 { *readFlags |= JSTICK_OK;
	   dp += 6;
	 }
	if (testRead)
	 { int i;
	   *readFlags |= TEST_OK;
	   for (i=0; i<testRead; i++)
	    { *dp++ = ((i&0x1) ? 0xaaaa : 0x5555);
	    }
	 }
	for (j=0; j<numADC; j++)
	 { if (adcOpen[j])
	    { getADC (j, dp);
	      dp++;
	    }
	 }
	for (j=0; j<numSlu; j++)
	 { if (sluOpen[j])
	    { short n = getSlu (j, &dp[1]);
	      *dp++ = n;
	      dp += (n+1)/2;
	    }
	 }
	return (dp - buffer);
}

/* DEBUG */
int delayit = 0;

rbtCommands (buffer, size)
unsigned short *buffer;
int size;
{
	register unsigned short *cmdp = buffer;
	unsigned short *top = (unsigned short *)&buffer[size];
	register short com;
       
	/* go through the entire command buffer, taking out each command
	   and acting on it. Values associated with the command buffer
	   are removed if necessary */

/* BEGIN DEBUG */
	if (delayit)
	 { int i;
	   for (i=0; i<delayit; i++)
	      ;	   
	 }
/* END DEBUG */

	while (cmdp < top)
	 { com = *cmdp++;

	   if ((com & ~JNT_MASK_C) == MOVE_ENC_C)
	    { if (putj (POSET, *cmdp, com & JNT_MASK_C) < 0)
	       { harderror (SRV_TIMEOUT_E, 14);
	       }
	      encset[com & JNT_MASK_C] = *cmdp;
	      cmdp++;
	    }
	   else if ((com & ~JNT_MASK_C) == MOVE_CUR_C)
	    { if (putj (DACSET, *cmdp, com & JNT_MASK_C) < 0)
	       { harderror (SRV_TIMEOUT_E, 15);
	       }
	      cmdp++;
	    }
	   else if ((com & ~JNT_MASK_C) == LOAD_ENC_C)
	    { register short j = com & JNT_MASK_C;
	      if (putj (STOPM, *cmdp, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 16);
	       }
	      encset[j] = *cmdp;
	      markFound &= ~(1<<j);
	      jntmode[j] &= ~MLOOKZI;
	      cmdp++;
	    }
	   else if ((com & ~JNT_MASK_C) == STOP_JNT_C)
	    { if (putj (STOPP, 0, com & JNT_MASK_C) < 0)
	       { harderror (SRV_TIMEOUT_E, 17);
	       }
	      encset[com & JNT_MASK_C] = encread[com & JNT_MASK_C];
	    }
	   else if ((com & ~CHAN_MASK_C) == PUT_PIO_C)
	    { putPio (com & CHAN_MASK_C, *cmdp);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == SET_PIO_C)
	    { unsigned short mask;
	      int chan;

	      mask = *cmdp;
	      chan = com & CHAN_MASK_C;
	      putPio (chan, getPioOut(chan) | mask);
	      printf ("set pio 0x%x, put=0x%x\n",mask,getPioOut(chan)|mask);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == CLEAR_PIO_C)
	    { unsigned short mask;
	      int chan;

	      mask = *cmdp;
	      chan = com & CHAN_MASK_C;
	      putPio (chan, getPioOut(chan) & ~mask);
	      printf("clear pio 0x%x, put=0x%x\n",mask,getPioOut(chan)& ~mask);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == SET_DAC_C)
	    { putDAC (com & CHAN_MASK_C, *cmdp);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == SET_SLU_C)
	    { putSlu (com & CHAN_MASK_C, &cmdp[1], *cmdp);
	      cmdp += ((*cmdp+1)/2 + 1);
	    }
	   else
	    { switch (com)
	       { case STOP_ALL_C:
		  { register short j;

		    for (j=0; j<PUMAJ; j++)
		     { if (putj (STOPP, 0, j) < 0)
			{ harderror (SRV_TIMEOUT_E, 18);
			}
		     }
		    break;
		  }
	         case HAND_POS_C:
		  { setHndPos (*cmdp);
		    cmdp++;
		    break;
		  }
	         case HAND_FORCE_C:
		  { setHndForce (*cmdp);
		    cmdp++;
		    break;
		  }
	         case SET_RATE_C:
		  { short interval, offset;

		    interval = *cmdp++;
		    offset = *cmdp++;
		    if (setRate (interval, offset) < 0)
		     { rbtError = MPR_BADRATE_E;
		       return (-1);
		     }
		    setRobotRate(interval);
		    break;
		  }
	         case SET_RUNREQ_C:
		  { maxRunReqs = *cmdp++;
		    break;
		  }
	         case SET_PARAM_C:
		  { register short j, k;
		    unsigned short mask;
		    short param;

		    for (k = (*cmdp++ - 1); k >= 0; k--)
		     { param = *cmdp++;
		       j = 0;
		       for (mask = *cmdp++; mask; mask >>= 1)
			{ if (mask & 1)
			   { if (putj (SETPAR, param, j) < 0)
			      { harderror(SRV_TIMEOUT_E, 19);
			      }
			   }
			  j++;
			}
		     }
		    break;
		  }
	         case SET_INTEGRATION_C:
		  { register short j;
		    unsigned short integrate = *cmdp++;

		    for (j=0; j<PUMAJ; j++)
		     { if (integrate & 1)
			{ setmode (MEINTBD, j);
			}
		       else
			{ clearmode (MEINTBD, j);
			}
		       integrate >>= 1;
		     }
		    break;
		  }
	         case SET_CALMODE_C:
		  { register short j;
		    unsigned short calibmode = *cmdp++;

		    for (j=0; j<PUMAJ; j++)
		     { if (calibmode & 1)
			{ setmode (MLOOKZI, j);
			}
		       else
			{ clearmode (MLOOKZI, j);
			}
		       calibmode >>= 1;
		     }
		    break;
		  }
		 case SET_CALFLAG_C:
		  {
		    if (*cmdp++)
		     { calibrated = 1;
		     }
		    else
		     { calibrated = 0;
		     }
		    break;
		  }
	         case ON_POWER_C:
		  { 
/*		    if (powerChan != -1)*/
		     { powerOn();
		     }
		    break;
		  }
	         case OFF_POWER_C:
		  { 
/*		    if (powerChan != -1)*/
		     { powerOff();
		     }
		    break;
		  }
	         case TORQUE_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!torqueRead)
		        { torqueRead = 1;
		          startTorque();
			}
		     }
		    else
		     { if (torqueRead)
			{ torqueRead = 0;
			  stopTorque();
			}
		     }
		    break;
		  }
	         case POT_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!potRead)
			{ potRead = 1;
			}
		     }
		    else
		     { if (potRead)
		        { potRead = 0;
		        }
		     }
		    break;
		  }
	         case PENDNT_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!pendantRead)
		        { pendantRead = 1;
#if VERBOSE
			  printf ("start pendant\n");
#endif
		          startPendant();
			}
		     }
		    else
		     { if (pendantRead)
			{ pendantRead = 0;
#if VERBOSE
			  printf ("stop pendant\n");
#endif
			  stopPendant();
			}
		     }
		    break;
		  }
	         case FORCE_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!forceRead)
		        { forceRead = 1;
		          startForce();
			}
		     }
		    else
		     { if (forceRead)
			{ forceRead = 0;
			  stopForce();
			}
		     }
		    break;
		  }
	         case JSTICK_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!jstickRead)
		        { jstickRead = 1;
		          startJstick();
			}
		     }
		    else
		     { if (jstickRead)
			{ jstickRead = 0;
			  stopJstick();
			}
		     }
		    break;
		  }
	         case TEST_READ_C:
		  { testRead = *cmdp++;
		    if (testRead < 0)
		     { testRead = 0;
		     }
		    else if (testRead > 64)
		     { testRead = 64;
		     }
		    break;
		  }
	         case PUT_TEST_C:
		  { cmdp++;
		    break;
		  }
	         case ADC_CHAN_C:
		  { register short k;
		    unsigned short open = *cmdp++;
		
		    for (k=0; k<numADC; k++)
		     { if (open & 1)
			{ adcOpen[k] = 1;
			}
		       else
			{ adcOpen[k] = 0;
			}
		       open >>= 1;
		     }
		    break;
		  }
	         case SLU_CHAN_C:
		  { register short k;
		    unsigned short open = *cmdp++;
		
		    for (k=0; k < numSlu; k++)
		     { if (open & 1)
			{ if (!(sluOpen[k]))
			   { sluOpen[k] = 1;
			     sluStart (k);
			   }
			}
		       else
			{ if (sluOpen[k])
			   { sluOpen[k] = 0;
			     sluStop (k);
			   }
			}
		       open >>= 1;
		     }
		    break;
		  }
	         case SET_PENDANT_C:
		  { if ((*cmdp & 0xff00) == M_NULL)
		     { *cmdp = ((*cmdp & 0x00ff) | pendantMsgRun);
		     }
		    /* see comment over last call to setPendant */
		    if (*cmdp & 0xff)
	    	     { setPendant ((calibrated ? 0 : CAL_LED) | *cmdp);
		     }
		    else
		     { setPendant ((calibrated ? NULL_LED : CAL_LED) | *cmdp);
		     }
	            cmdp++;
		    break;
		  }
	         case SET_MAINT_COM_C:
		  {
		    maintCom = *cmdp++;
		    break;
		  }
	         case END_C:	/* now ignored */
		  {
		    break;
		  }
		 default:
		  {
		    rbtError = HST_BADCMD_E;
		    return (-1);
		    break;
		  }
	       }
	    }
	 }
	return (1);
}

rbtExit ()
{
	powerOff();
}

int interval;
int offset;
int maxRunReqs;
int new = 1;

setRate (i, o)
unsigned i, o;
{
	int numRbtCycles, logCycles, j;

	if (i > ((unsigned)~0/10))
	 { return (-1);
	 }
	i *= 10;
	if (i < clockRate || i < jointRate)
	 { return (-1);
	 }
	interval = i;
	offset = o*10;
	new = 1;

	return (1);
}

static int log2 (k)
int k;
{
	int l;

	l = 1;
	for ( ; k & ~3; k >>= 1)
	 { l++;
	 }
	if (k == 3)
	 { l++;
	 }
	return (l);
}

setBaseRate ()
{
	interval = clockRate;
	offset = 0;
	new = 1;
}

getInterval ()
{
	long ticks;
	static long nexttime;
	long gettime();

	ticks = gettime();

	if (new)
	 { long time = ticks * clockRate + interval;
	   nexttime = time - (time - offset) % interval;
	   new = 0;
	 }
	else
	 { nexttime += interval;
	 }
	return ((nexttime + (clockRate/2))/clockRate - ticks);
}

getMaxDelay()
{
#define WORST_RESPONSE	100	/* the worst delay overhead expected */

	return ((interval/clockRate)*(maxRunReqs+2)+WORST_RESPONSE);
}

rbtIdle()
{
	int j;

	for (j=0; j<PUMAJ; j++)
	 { if (getj (JSTAT, &jstat[j], j) < 0)
	    { harderror (SRV_TIMEOUT_E, 20);
	    }
	 }
	
	servicePendant();

	if (!(armPowerOn()))
	 { unsigned short value;
	   for (j=0; j<PUMAJ; j++)
	    { if (getj (READ, &value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 21);
	       }
	      encread[j] = value;
	      if (putj (POSET, value, j) < 0)
	       { harderror (SRV_TIMEOUT_E, 21);
	       }
	      encset[j] = value;
	    }
	   ereadOk = 1;
	 }
}

setmode (value, j)
unsigned short value, j;
{
	if ((jntmode[j] & value) != value)
  	 { jntmode[j] |= value; 
	   if (putmode (jntmode[j], j) < 0)
	    { harderror (SRV_TIMEOUT_E, 22);
	    }
	 }
}

clearmode (value, j)
unsigned short value, j;
{
	if ((jntmode[j] & value) != 0)
	 { jntmode[j] &= ~value; 
	   if (putmode (jntmode[j], j) < 0)
	    { harderror (SRV_TIMEOUT_E, 23);
	    }
	 }
}
