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

#include "../h/cdefs.h"
#include "../h/moper_com.h"
#include "robotsim.h"
#include "serialDev.h"
#include "manipIO.h"

#define JPL_TESTBED	1

/* Fake force and teleop data for the JPL testbed */

#define FORCE_DOF0 0.01
#define FORCE_DOF1 0.01
#define FORCE_DOF2 0.01
#define FORCE_DOF3 0.01
#define FORCE_DOF4 0.01
#define FORCE_DOF5 0.01

#define TELEOP_DOF0 0.1
#define TELEOP_DOF1 0.2
#define TELEOP_DOF2 0.1
#define TELEOP_DOF3 0.2
#define TELEOP_DOF4 0.1
#define TELEOP_DOF5 0.2
#define TELEOP_BUTTON 1
#define TELEOP_TRIGGER 123

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

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

union _int {
  int i;
  unsigned short s[2];
  };

union _float {
  float f;
  unsigned short s[2];
  };

static bigEndian;		/* machine byte order, set in rbtInit() */

/* Macros to help arrange proper byte order. The PUMA controller at the
   other end, is littleEndian (VAX-like). Bytes should be placed into
   and removed from the packing structure accordingly. */

#define BYTE0	(bigEndian ? 1 : 0)	
#define BYTE1	(bigEndian ? 0 : 1)

#define RETURN_ERROR(err,loc)		\
	r->cs.rbtError = (err);		\
	r->cs.errorLoc = (loc);		\
	return (-1);

static int log2 ();
static testForBigEndian();
static swapBuffer ();

rbtInit(r, precal)
register RobotDesc *r;
int precal;
{
	/* called by moper when it is first started up (loaded) */

	int j;

	bigEndian = testForBigEndian();

	r->cs.clockRate = clockSetup(r);
	r->cs.jointRate = getJointRate(r);
	r->cs.intervalJustSet = 1;
	r->cs.moperID = 100;

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

	if (!precal)
	 { for (j=0; j<r->jls->numJ; j++)
	    { if (putj (r, STOPM, 0100000, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 11);
	       }
	      r->cs.encset[j] = 0100000;
	    }
	   r->cs.calibrated = 0;
	 }
	else
	 { r->cs.calibrated = 1;
	 }

	initJstick(r);
	initForce(r);

	return (1);
}

rbtSetup(r)
register RobotDesc *r;
{
	/* called by moper before every new control cycle session */

	int j;

	for (j=0; j<r->jls->numJ; j++)
	 { unsigned short stat;
	   unsigned short value;
	   
	   if (getj (r, JSTAT, &stat, j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 12);
	    }
	   if (stat & JFNDIXS)
	    { if (getj (r, READ, &value, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 13);
	       }
	      if (putj (r, STOPM, value, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 14);
	       }
	    }
	   if (putmode (r, MESERVO, j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 15);
	    }
	   r->cs.jmode[j] = MESERVO;
	   r->cs.integration = 0;
	   r->cs.calibMode = 0;
	   r->cs.markFound = 0;
	   r->cs.oldMarkFound = 0;
	 }

	if (!r->cs.pendantRead)
	 { startPendant(r);
	   r->cs.pendantRead = 1;
	 }
	r->cs.jstickRead = 0;
	stopJstick(r);
	r->cs.forceRead = 0;
        r->cs.teleopRead = 0;
	r->cs.testRead = 0;
	stopForce(r);

	return (1);
}

rbtStart(r, buffer)
register RobotDesc *r;
unsigned short *buffer;
{
	unsigned short *dp = buffer;
	union _pack pack;

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

	*dp++ = 0x5555;
	*dp++ = 0xaaaa;
	*dp++ = r->cs.robotID;	
	*dp++ = r->cs.moperID;
	*dp = 0;
	*dp |= r->sio.torqueAvail ? 	0x01 : 0;
	*dp |= r->sio.potsAvail ?	0x02 : 0;
	*dp |= r->sio.pendantAvail ? 	0x04 : 0;
	*dp |= r->sio.forceAvail ? 	0x08 : 0;
	*dp |= r->sio.jstickAvail ? 	0x10 : 0;
	*dp |= r->sio.teleopAvail ? 	0x40 : 0;
	*dp++;

	pack.b[BYTE0] = r->sio.numADC;
	pack.b[BYTE1] = r->sio.numDAC;
	*dp++ = pack.s;
	pack.b[BYTE0] = r->sio.numPioIn;
	pack.b[BYTE1] = r->sio.numPioOut;
	*dp++ = pack.s;
	pack.b[BYTE0] = r->sio.numSlu;
	pack.b[BYTE1] = r->cs.integration;
	*dp++ = pack.s;
	pack.b[BYTE0] = r->cs.calibMode;
	pack.b[BYTE1] = r->cs.markFound;
	*dp++ = pack.s;

#if READ_ENCSET

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

	   if (getj (r, READ, &value, j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 16);
	    }
	   diff = value - r->cs.encset[j];
	   if (ABS(diff) > 500)
	    { if (putj (r, POSET, value, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 17);
	       }
	      r->cs.encset[j] = value;
	    }
	   *dp++ = r->cs.encset[j];
	 }
#endif
	return (dp - buffer);
}

rbtIcton(r, rcnt, rids, buffer, size)
register RobotDesc *r;
int *rcnt;
unsigned short **rids;
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 */

	r->cs.maxRunReqs = -1;
	r->cs.opFlags = -1;
	if (rcnt)
	 { *rcnt = 0;	/* set default value */
	 }

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

	   if ((com & ~0xf) == ROBOT_IDS_C)
	    { *rcnt = (com & 0xf);
	      *rids = cmdp;
	      cmdp += *rcnt;
	      break;
	    }
	   else
	    { switch (com)
	       { case SET_RATE_C:
		  { interval = *cmdp++;
		    offset = *cmdp++;
		    break;
		  }
		 case SET_RUNREQ_C:
		  { r->cs.maxRunReqs = *cmdp++;
		    break;
		  }
		 case START_C:
		  { r->cs.opFlags = *cmdp++;
		    break;
		  }
		 case END_C:	/* now ignored */
		  { 
		    break;
		  }
		 case OFF_POWER_C:
		  { break;
		  }
		 default:
		  { RETURN_ERROR (HST_BADCMD_E, 18);
		  }
	       }
	    }
	 }
	if (interval < 0 || offset < 0 || 
	    r->cs.opFlags < 0 || r->cs.maxRunReqs < 0)
	 { RETURN_ERROR (HST_BADCMD_E, 19);
	 }
	if (setRate (r, interval, offset) < 0)
	 { setBaseRate (r);
	   RETURN_ERROR (MPR_BADRATE_E, 40);
	 }
	setRobotRate(r, interval);

	return (1);
}

rbtGather (r, buffer)
register RobotDesc *r;
unsigned short *buffer;
{
	unsigned short *dp = buffer;
	unsigned short *readFlags;
	register short j;
	unsigned short state;
        union _float force[6];
        union _float teleop[6];
	int numJ;

	numJ = r->jls->numJ;

	if (j = armError(r))
	 { RETURN_ERROR (j, 41);
	 }

	for (j=0; j<numJ; j++)
	 { if (getj (r, JSTAT, &r->cs.jstat[j], j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 20);
	    }
	 }

	servicePendant(r);
	for (j = 0; j<numJ; j++)
	 { if (getj (r, READ, dp, j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 21);
	    }
	   r->cs.encread[j] = *dp;
	   dp++;
	 }

	*dp++ = getHandPos(r);
	*dp++ = getHandForce(r);

	state = 0;
	if (armPowerOn(r))
	 { state |= POWER_ON;
	 }
	if (r->cs.calibrated)
	 { state |= IS_CALIBRATED;
	 }
	*dp++ = state;

	readFlags = dp++;
	*readFlags = 0;

	for (j=0; j<r->sio.numPioIn; j++)
	 { *dp++ = getPioIn (r, j);
	 }
	for (j=0; j<r->sio.numPioOut; j++)
	 { *dp++ = getPioOut (r, j);
	 }
	for (j=0; j<numJ; j++)
	 { if (r->cs.jmode[j] & MLOOKZI && !(r->cs.markFound & (1<<j)))
	    { unsigned short stat;
	      if (getj (r, JSTAT, &stat, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 22);
	       }
	      if (stat & JFNDIXS)
	       { r->cs.markFound |= (1<<j);
		 putchar ('1'+j);
	       }
	    }
	 }
	if (r->cs.markFound != r->cs.oldMarkFound)
	 { *readFlags |= MARK_FOUND;
	   *dp++ = r->cs.markFound;
	   r->cs.oldMarkFound = r->cs.markFound;
	 }
	if (r->cs.torqueRead)
	 { getTorque (r, dp);
	   dp += 6;
	   *readFlags |= TORQUE_OK;
	 }
	if (r->cs.potRead)
	 { for (j=0; j<numJ; j++)
	    { if (getj (r, READP, dp, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 23);
	       }
	      *dp++ &= 0x3ff;
	    }
	   *readFlags |= POT_OK;
	 }
	if (r->cs.pendantRead && getPendant (r, dp))
	 { *readFlags |= PENDNT_OK;
	   dp += 3;
	 }
	if (r->cs.forceRead && getForce (r,dp))
	 { *readFlags |= FORCE_OK;
#if JPL_TESTBED
	   *readFlags |= FORCE_DATA_FLOAT;
           force[0].f = FORCE_DOF0;
           force[1].f = FORCE_DOF1;
           force[2].f = FORCE_DOF2;
           force[3].f = FORCE_DOF3;
           force[4].f = FORCE_DOF4;
           force[5].f = FORCE_DOF5;
           for (j=0;j<6;j++) {
            *dp++ = force[j].s[0];
            *dp++ = force[j].s[1];
           }
#else
	   dp += 6;
#endif
	 }
	if (r->cs.teleopRead && r->sio.teleopAvail)
	 { *readFlags |= TELEOP_OK;
#if JPL_TESTBED
           teleop[0].f = TELEOP_DOF0;
           teleop[1].f = TELEOP_DOF1;
           teleop[2].f = TELEOP_DOF2;
           teleop[3].f = TELEOP_DOF3;
           teleop[4].f = TELEOP_DOF4;
           teleop[5].f = TELEOP_DOF5;
           for (j=0;j<6;j++) {
            *dp++ = teleop[j].s[0];
            *dp++ = teleop[j].s[1];
           }
           *dp++ = TELEOP_BUTTON;
           *dp++ = TELEOP_TRIGGER;
#else
	   dp += 6;
#endif
	 }
	if (r->cs.testRead)
	 { int i;
	   for (i=0; i<r->cs.testRead; i++)
	    { *dp++ = ((i&0x1) ? 0xaaaa : 0x5555);
	    }
	 }
	if (r->cs.jstickRead && getJstick (r,dp))
	 { *readFlags |= JSTICK_OK;
	   dp += 6;
	 }
	for (j=0; j<r->sio.numADC; j++)
	 { if (r->cs.adcOpen[j])
	    { getADC (r, j, dp);
	      dp++;
	    }
	 }
	for (j=0; j<r->sio.numSlu; j++)
	 { if (r->cs.sluOpen[j])
	    { short n = getSlu (r, j, &dp[1], 256);
	      *dp++ = n;
	      if (bigEndian)
	       { swapBuffer(dp, (n+1)/2);
	       }
	      dp += (n+1)/2;
	    }
	 }
	return (dp - buffer);
}

rbtCommands (r, buffer, size)
register RobotDesc *r;
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 */

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

	   if ((com & ~JNT_MASK_C) == MOVE_ENC_C)
	    { if (putj (r, POSET, *cmdp, com & JNT_MASK_C) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 24);
	       }
	      r->cs.encset[com & JNT_MASK_C] = *cmdp;
	      cmdp++;
	    }
	   else if ((com & ~JNT_MASK_C) == MOVE_CUR_C)
	    { if (putj (r, DACSET, *cmdp, com & JNT_MASK_C) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 25);
	       }
	      cmdp++;
	    }
	   else if ((com & ~JNT_MASK_C) == LOAD_ENC_C)
            { register short j = com & JNT_MASK_C;
	      if (putj (r, STOPM, *cmdp, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 26);
	       }
	      r->cs.encset[j] = *cmdp;
	      cmdp++;
	      r->cs.markFound &= ~(1<<j);
	      r->cs.jmode[j] &= ~MLOOKZI;
	    }
	   else if ((com & ~JNT_MASK_C) == STOP_JNT_C)
	    { if (putj (r, STOPP, 0, com & JNT_MASK_C) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 27);
	       }
	      r->cs.encset[com & JNT_MASK_C] = r->cs.encread[com & JNT_MASK_C];
	    }
	   else if ((com & ~CHAN_MASK_C) == PUT_PIO_C)
	    { putPio (r, 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 (r, chan, getPioOut(r, 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 (r, chan, getPioOut(r, chan) & ~mask);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == SET_DAC_C)
	    { putDAC (r, com & CHAN_MASK_C, *cmdp);
	      cmdp++;
	    }
	   else if ((com & ~CHAN_MASK_C) == SET_SLU_C)
	    { int size;
	      size = *cmdp++;
	      if (bigEndian)
	       { swapBuffer (cmdp, (size+1)/2);
	       }
	      putSlu (r, com & CHAN_MASK_C, cmdp, size);
	      cmdp += (size+1)/2;
	    }
	   else
	    { switch (com)
	       { case STOP_ALL_C:
		  { register short j;

		    for (j=0; j<r->jls->numJ; j++)
		     { if (putj (r, STOPP, 0, j) < 0)
			{ RETURN_ERROR (SRV_TIMEOUT_E, 28);
			}
		     }
		    break;
		  }
	         case HAND_POS_C:
		  { setHandPos (r, *cmdp);
		    cmdp++;
		    break;
		  }
	         case HAND_FORCE_C:
		  { setHandForce (r, *cmdp);
		    cmdp++;
		    break;
		  }
	         case SET_RATE_C:
		  { int interval, offset;

		    interval = *cmdp++;
		    offset = *cmdp++;
		    if (setRate (r, interval, offset) < 0)
		     { RETURN_ERROR (MPR_BADRATE_E, 42);
		     }
		    setRobotRate(r, interval);
		    break;
		  }
	         case SET_RUNREQ_C:
		  { r->cs.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 (r, SETPAR, param, j) < 0)
			      { RETURN_ERROR (SRV_TIMEOUT_E, 29);
			      }
			   }
			  j++;
			}
		     }
		    break;
		  }
	         case SET_INTEGRATION_C:
		  { register short j;
		    unsigned short integrate = *cmdp++;

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

		    for (j=0; j<r->jls->numJ; j++)
		     { if (calibmode & 1)
			{ if (setmode (r, MLOOKZI, j) < 0)
			   { return (-1);
			   }
			}
		       else
			{ if (clearmode (r, MLOOKZI, j) < 0)
			   { return (-1);
			   }
			}
		       calibmode >>= 1;
		     }
		    break;
		  }
		 case SET_CALFLAG_C:
		  {
		    if (*cmdp++)
		     { r->cs.calibrated = 1;
		     }
		    else
		     { r->cs.calibrated = 0;
		     }
		    break;
		  }
	         case ON_POWER_C:
		  { 
		    powerOn(r);
		    break;
		  }
	         case OFF_POWER_C:
		  { 
		    powerOff(r);
		    break;
		  }
	         case TORQUE_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.torqueRead)
		        { r->cs.torqueRead = 1;
		          startTorque(r);
			}
		     }
		    else
		     { if (r->cs.torqueRead)
			{ r->cs.torqueRead = 0;
			  stopTorque(r);
			}
		     }
		    break;
		  }
	         case POT_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.potRead)
			{ r->cs.potRead = 1;
			}
		     }
		    else
		     { if (r->cs.potRead)
		        { r->cs.potRead = 0;
		        }
		     }
		    break;
		  }
	         case PENDNT_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.pendantRead)
		        { r->cs.pendantRead = 1;
			  printf ("start pendant\n");
		          startPendant(r);
			}
		     }
		    else
		     { if (r->cs.pendantRead)
			{ r->cs.pendantRead = 0;
			  printf ("stop pendant\n");
			  stopPendant(r);
			}
		     }
		    break;
		  }
#if JPL_TESTBED
	         case TELEOP_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.teleopRead)
		        { r->cs.teleopRead = 1;
                          printf("READ TELEOP ON\n"); 
			}
		     }
		    else
		     { if (r->cs.teleopRead)
			{ r->cs.teleopRead = 0;
                          printf("READ TELEOP OFF\n"); 
			}
		     }
		    break;
		  }
#endif
	         case FORCE_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.forceRead)
		        { r->cs.forceRead = 1;
		          startForce(r);
                          printf("READ FORCE ON\n"); 
			}
		     }
		    else
		     { if (r->cs.forceRead)
			{ r->cs.forceRead = 0;
			  stopForce(r);
                          printf("READ FORCE OFF\n"); 
			}
		     }
		    break;
		  }
	         case JSTICK_READ_C:
		  { 
		    if (*cmdp++)
		     { if (!r->cs.jstickRead)
		        { r->cs.jstickRead = 1;
		          startJstick(r);
			}
		     }
		    else
		     { if (r->cs.jstickRead)
			{ r->cs.jstickRead = 0;
			  stopJstick(r);
			}
		     }
		    break;
		  }
	         case TEST_READ_C:
		  { r->cs.testRead = *cmdp++;
		    if (r->cs.testRead < 0)
		     { r->cs.testRead = 0;
		     }
		    else if (r->cs.testRead > 64)
		     { r->cs.testRead = 64;
		     }
		    break;
		  }
	         case PUT_TEST_C:
		  { cmdp++;
		    break;
		  }
	         case ADC_CHAN_C:
		  { register short k;
		    unsigned short open = *cmdp++;
		
		    for (k=0; k<r->sio.numADC; k++)
		     { if (open & 1)
			{ r->cs.adcOpen[k] = 1;
			}
		       else
			{ r->cs.adcOpen[k] = 0;
			}
		       open >>= 1;
		     }
		    break;
		  }
	         case SLU_CHAN_C:
		  { register short k;
		    unsigned short open = *cmdp++;
		
		    for (k=0; k < r->sio.numSlu; k++)
		     { if (open & 1)
			{ if (!(r->cs.sluOpen[k]))
			   { r->cs.sluOpen[k] = 1;
			     sluStart (r, k);
			   }
			}
		       else
			{ if (r->cs.sluOpen[k])
			   { r->cs.sluOpen[k] = 0;
			     sluStop (r, k);
			   }
			}
		       open >>= 1;
		     }
		    break;
		  }
	         case SET_PENDANT_C:
		  {
		    setPendant (r,*cmdp);
	            cmdp++;
		    break;
		  }
	         case END_C:
		  {
		    break;
		  }
		 case PUT_VECTOR_C:
		  { union _float {
			float f;
			short s[2];
		      } fvalue;
		    int size;
		    int j;

		    size = *cmdp++;
		    for (j=0; j<size; j++)
		     { fvalue.s[0] = *cmdp++;
		       fvalue.s[1] = *cmdp++;
		       /* goes into the bit bucket for now */
		     }		    
		  }
		 default:
		  {
		    printf ("bad command from host: 0x%x\n", com);
		    RETURN_ERROR (HST_BADCMD_E, 43);
		    break;
		  }
	       }
	    }
	 }
	return (1);
}

rbtExit(r)
RobotDesc *r;
{
	powerOff(r);
	return (1);
}

setRate (r, i, o)
RobotDesc *r;
int i, o;
{
	if (i > ((unsigned)~0/10))
	 { return (-1);
	 }
	i *= 10;
	if (i < r->cs.clockRate || i < r->cs.jointRate)
	 { return (-1);
	 }

	r->cs.interval = i;
	r->cs.offset = o*10;
	r->cs.intervalJustSet = 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 (r)
RobotDesc *r;
{
	r->cs.interval = r->cs.clockRate;
	r->cs.offset = 0;
	r->cs.intervalJustSet = 1;
}

getInterval (r)
RobotDesc *r;
{
	long ticks;
	static long nexttime;
	int gettime();

	ticks = gettime(r);

	if (r->cs.intervalJustSet)
	 { long time = ticks * r->cs.clockRate + r->cs.interval;
	   nexttime = time - (time - r->cs.offset) % r->cs.interval;
	   r->cs.intervalJustSet = 0;
	 }
	else
	 { nexttime += r->cs.interval;
	 }
	return ((nexttime + (r->cs.clockRate/2))/r->cs.clockRate - ticks);
}

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

	return ((r->cs.interval/r->cs.clockRate)*
		(r->cs.maxRunReqs+2)+WORST_RESPONSE);
}


rbtIdle(r)
RobotDesc *r;
{
	int j;

	for (j=0; j<r->jls->numJ; j++)
	 { if (getj (r, JSTAT, &r->cs.jstat[j], j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 30);
	    }
	 }
	
	servicePendant(r);

	if (!(armPowerOn(r)))
	 { for (j=0; j<r->jls->numJ; j++)
	    { if (putj (r, STOPP, 0, j) < 0)
	       { RETURN_ERROR (SRV_TIMEOUT_E, 31);
	       }
	    }
	 }
	return (1);
}

setmode (r, value, j)
RobotDesc *r;
short value, j;
{
	if ((r->cs.jmode[j] & value) != value)
  	 { r->cs.jmode[j] |= value; 
	   if (putmode (r, r->cs.jmode[j], j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 32);
	    }
	 }
	return (1);
}

clearmode (r, value, j)
RobotDesc *r;
short value, j;
{
	if ((r->cs.jmode[j] & value) != 0)
	 { r->cs.jmode[j] &= ~value; 
	   if (putmode (r, r->cs.jmode[j], j) < 0)
	    { RETURN_ERROR (SRV_TIMEOUT_E, 33);
	    }
	 }
	return (1);
}

static testForBigEndian()
{
	union _pack swap;
	swap.s = 1;
	return (swap.b[1] == 1);
}

static swapBuffer (p, size)
unsigned short *p;
int size;
{
	union _pack swap;
	unsigned char b;
	register int i;

	for (i=0; i<size; i++)
	 { swap.s = p[i];
	   b = swap.b[0];
	   swap.b[0] = swap.b[1];
	   swap.b[1] = b;
	   p[i] = swap.s;
	 }
}
