/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	rcclCreate
 *
 * DESCRIPTION:	Contains the planning level RCCL control routines.
 *		
 *		These are the routines that create manipulators,
 *		turn the trajectory generator on and off, etc.
 * 
 * CAVEATS:	This code isn't particularly bad, but it is cumbersome,
 *		and could probably be streamlined.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------

	Basic memory layout between the planning task and the trajectory
	tasks:
  
		............................................               
		.               Planning Task              .<--+
                ............................................   |
                     ^                       ^		       |
                     |                       |                 |
		------------            ------------     ------------
	        |  shared  |            |  shared  |     |  global  |
		|  mem #1  |            |  mem #2  |     |  memory  |
		------------            ------------     ------------
		     |                      |                  |
                     |   +------------------|---+--------------+
                     v   v                  v   v
                ................        ................
                .  trajectory  .        .  trajectory  .
                .   task #1    .        .   task #2    .
                ................        ................


	Memory layout associated with a particular manipulator:
			 
	................       -----------------	................
	.	       .       |    Motion     |        .	       .
	.	       .<======|    Control    |=======>.	       .
	.	       .       |    Block      |	.	       .
	.	       .       -----------------	.	       .
	.	       .				.	       .
	.   PLANNING   .          Motion Queue          .  TRAJECTORY  .
	.     TASK     .    ------------------------	.    TASK      .
	.	       .    |	  |     |     |	   |    .	       .
	.	       .--->|     |	|     |    |--->.	       .
	.	       .    |     |     |     |    |    .	       .
	.	       .    ------------------------    .	       .
	................             ^			................
	       ^                   Motion		        ^
	       |		  Requests			|
	       v                                                v
	  ------------				           ------------
	  |  Motion  |                                     |  Motion  |
	  |  Value   |					   | Parameter|
	  |  Block   |				           |  Block   |
	  ------------				           ------------
 
----------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/errorCodes.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/allocMem.h"
#include "../h/rcclDefs.h"
#include "../h/robotMath.h"
#include "../h/vector.h"
#include "../h/assignMem.h"

#include "../h/rciConfig.h"		/* too get MAX_CPUS */

#include "../rci/link.h"
#include "../rccl/motionRequest.h"
#include "../rccl/setpoint.h"
#include "../rccl/motionQueue.h"
#include "../rccl/transform.h"
#include "../rccl/kynMacros.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"

#include "../h/timeval.h"

static int simflag;
static int availableCpus;
static int timeoutCount = 2;
static int scheduleDisp = UNDEF;
static int leavePowerOn = 0;

RCI_DESC *rcclTd[MAX_CPUS];
RCCL_SMEM *sharedMem[MAX_CPUS];

static int cpuUsage[MAX_CPUS];
extern RCCL_GMEM *rcclGmem;
static int manipsAllocated = 0;

static unsigned long taskMask = 0;
static int numRobots = 0;
static int pollingDev = UNDEF;		/* arbiter polling interrupt */

typedef struct _MANIP_DESC {
	/* Per-manipulator block containing things that the control
	   routines need to setup and control a particular manipulator.
	   This is mostly pointers to the various memory areas.
	 */
	struct _MANIP_DESC *prev;
	struct _MANIP_DESC *next;

	MANIP *mnp;
	MOTION_PARAMETER_BLOCK *mpb;
	MOTION_QUEUE *mq;
	MOTION_VALUE_BLOCK *mvb;
	MOTION_REQUEST *mr;
	MOTION_CONTROL_BLOCK *mcb;
} MANIP_DESC;

static MANIP_DESC manipHead;
static MANIP_DESC *manipList = &manipHead;
static MANIP_DESC *freeManips[MAX_CPUS];

int rcclErrorExitFlag;
float rcclDefaultSpeed = 1.0;
float rcclDefaultTransVel = DEFAULT_TRANS_VEL;
float rcclDefaultRotVel = DEFAULT_ROT_VEL * DEGTORAD;

static double releaseTime;
static envInitDone = 0;

static doCleanup();
static doInit();
static unsigned long tasksAreActive();
static doEnvInit();
static cpuSelect ();
static setpointTaskCreate ();
static MANIP_DESC *allocManipMemory ();
static setpointManipAttach ();
static setpointManipDetach ();
static freeManip ();
static addManipTableEntry ();
static deleteManipTableEntry ();
static newManipTable ();

static initVars()
{
	envInitDone = 0;
	simflag = 0;
	timeoutCount = 0;
	scheduleDisp = UNDEF;
	leavePowerOn = 0;
	bzero((char*)rcclTd, sizeof(rcclTd));
	bzero((char*)sharedMem, sizeof(sharedMem));
	rcclGmem = NULL;
	manipsAllocated = 0;
	taskMask = 0;
	numRobots = 0;
	pollingDev = UNDEF;
	manipList->next = manipList->prev = manipList;
	bzero((char*)freeManips, sizeof(freeManips));
	rcclErrorExitFlag = 0;
	rcclDefaultSpeed = 0.0;
	rcclDefaultTransVel = DEFAULT_TRANS_VEL;
	rcclDefaultRotVel = DEFAULT_ROT_VEL * DEGTORAD;
	releaseTime = 0.0;
}

static doCleanup()
{
	/* throw out the memory allocated for the MANIP_DESC list */

	MANIP_DESC *md, *md_next;

	for (md=manipList->next; md!=manipList; md=md_next)
	 { md_next = md->next;
	   FREE(md->mvb);
	   FREE(md->mr);
	   FREE(md);
	 }
	initVars();
}

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	initVars();
	setpVariableInit();
	requestExitFxn (doCleanup);
	requestFlagInit (&initDone);
}

rcclExitOnError(n)
int n;
{
printf ("Use rcclSetOptions(RCCL_ERROR_EXIT) instead of rcclExitOnError()\n");
	rcclSetOptions (RCCL_ERROR_EXIT);
}

MANIP *rcclCreate (name, cpuMask)
const char *name;
unsigned long cpuMask;
{
	/* The start of the world for most RCCL programmers.

	   Sets up all the structures necessary to control the
	   specified manipulator.
	 */
	   
	char *fxnName = "rcclCreate()";
	MANIP_DESC *md;
	int cpu;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	MASK_RCI_AUTO_ABORT();

	/* Make sure no RCCL tasks are active */	

	if (tasksAreActive())
	 { errorMsg (ETaskActive, rcclIsActiveMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }

	/* Initialize if necessary */

	if (!envInitDone)
	 { if (doEnvInit() < 0)
	    { errorMsg (getErrorCode(0), rcclCantInitMsg, fxnName);
	      RETURN_ERROR (NULL);
	    }
	 }

	/* Check the list of manip structures to see if we have 
	   a trajectory generator with this name
	 */

	for (md = manipList->next; md != manipList; md = md->next)
	 { if (strcmp (md->mnp->name, name) == 0)
	    { errorMsg (EDuplicate, rcclDuplicateMsg, fxnName, name);
	      RETURN_ERROR (NULL);
	    }
	 }

	/* Choose a cpu for this trajectory generator to run on.
	   It there is no setpoint task running on that CPU,
	   set one up.
	 */

	if ((cpu = cpuSelect (cpuMask)) < 0)
	 { errorMsg (ENotAvail, rcclNoCpuMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }

	/* Create an RCCL task on this CPU if necesary */

	if (cpuUsage[cpu] == UNDEF)
	 { if (setpointTaskCreate (cpu) < 0)
	    { errorMsg (getErrorCode(0), rcclTaskSetupMsg, fxnName);
	      RETURN_ERROR (NULL);
	    }
	   cpuUsage[cpu] = 0;
	 }

	/* The plan is that we allocate memory permanently. We keep
	   all the pointers to the various memory areas around
	   in the MANIP_DESC structure. If we release the manipulator,
	   we simply tuck the memory away for later use. If we
	   create 1 more manipulator than was previously created
	   then we allocate new memory.
	*/

	if (freeManips[cpu] == NULL)
	 { if ((md = allocManipMemory(cpu)) == NULL)
	    { errorMsg (getErrorCode(0), rcclAllocMemMsg, fxnName, name);
	      RETURN_ERROR (NULL);
	    }
	 }
	else
	 { md = freeManips[cpu];
	   freeManips[cpu] = md->next;
	 }

	/* Initialize the memory structures, attach to physical
	   robot, let the trajectory generator know about us ... */

	if (setpointManipAttach (md, name, cpu) < 0)
	 { errorMsg (getErrorCode(0), rcclTaskSetupMsg, fxnName);
	   md->next = freeManips[cpu];	      
	   freeManips[cpu] = md;
	   RETURN_ERROR (NULL);
	 }

	LINK (md, manipList);	/* close the deal! */
	cpuUsage[cpu]++;

	RETURN_NORMAL (md->mnp);
}

rcclDelete (mnp)
MANIP *mnp;
{
	/* Called to 'put a manipulator' away. Note that memory
	   resources are never actually freed, although they are
	   squirelled away for use with manipulators allocated later.
	 */

	MANIP_DESC *md;
	int cpu;
	char *fxnName = "rcclDelete()";

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();

	/* Make sure all tasks are released */

	if (tasksAreActive())
	 { errorMsg (ETaskActive, rcclIsActiveMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	/* Make sure that the manip object is there */

	for (md = manipList->next; md != manipList; md = md->next)
	 { if (md->mnp == mnp)
	    { break;
	    }
	 }
	if (md == manipList)
	 { RETURN_NORMAL (0);
	 }

	cpu = mnp->cpu;

	UNLINK (md);
	cpuUsage[cpu]--;

	if (setpointManipDetach (md) < 0)
	 { errorMsg (getErrorCode(0), rcclTaskSetupMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	md->next = freeManips[cpu];
	freeManips[cpu] = md;

	RETURN_NORMAL (1);	
}

static unsigned long tasksAreActive()
{
	/* Returns a bitmask of the cpus on which an RCCL task is active */

	unsigned long mask = 0;
	register int k;

	for (k=0; k<MAX_CPUS; k++)
	 { if (rcclTd[k] && rcclTd[k]->tcb->controlMode == ACTIVE)
	    { mask |= (1<<k);
	    }
	 }
	return (mask);
}

#define pset	"RCCL"		/* where we get RCCL params from */

static doEnvInit()
{
	/* Called once per program to set some things up. Mainly,
	   it reads the .rciparams files and sets up some global
	   shared memory.
	*/

	char *fxnName = "doEnvInit()";
	
	char *schedStr;
	int par;
	int status;
	int interval;
	int count;
	int k;
	float serviceDelay;
	char *simName;

	for (k=0; k<MAX_CPUS; k++)
	 { cpuUsage[k] = UNDEF;
	 }
	manipList->next = manipList->prev = manipList;

	/* Check for the following keywords in the .rciparams file:

	   cpus: 	which CPUs can we try an run trajectory tasks on
	   interval:	control cycle interval, in msec.
	   speed:	speed scaling factor.
	   tvel:	default translational velocity
	   rvel:        default rotational velocity
	   simulate:	should we run in simulator mode? May also identify
			the name of a simulator program.
	   timeout:	#of control cycles to wait before timing out
	   scheduling:	trajectory task scheduling discipline.
	   leavePowerOn: leave the robot power on after the program exits.

	   If any of these are present, use their values appropriately.
	*/
	
	if ((par = rciParameters (pset, 0)) < 1)
	 { errorMsg (ENotDefined, rcclNoParamSetMsg, fxnName, pset);
	   return (-1);
	 }
	if (getparInteger (&availableCpus, "cpus", par) < 1)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "cpus");
	   return (-1);
	 }
	if (getparInteger (&interval, "interval", par) < 1)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "interval");
	   return (-1);
	 }

	if ((status = getparFloat (&rcclDefaultSpeed, "speed", par)) < 0)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "speed");
	   return (-1);
	 }
	if (status == 0)
	 { rcclDefaultSpeed = 1.0;
	 }
	else if (rcclDefaultSpeed <= 0.0)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "speed");
	   return (-1);
	 }
	else
	 { printf ("default speed set to %g\n", rcclDefaultSpeed);
	 }

	/* get default translational velocity */

	if ((status = getparFloat (&rcclDefaultTransVel, "tvel", par)) < 0)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "tvel");
	   return (-1);
	 }
	if (status == 0)
	 { rcclDefaultTransVel = DEFAULT_TRANS_VEL;
	 }
	else 
	 { rcclDefaultTransVel = FABS(rcclDefaultTransVel);
	 }

	/* get default rotational velocity */

	if ((status = getparFloat (&rcclDefaultRotVel, "rvel", par)) < 0)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "rvel");
	   return (-1);
	 }
	if (status == 0)
	 { rcclDefaultRotVel = DEFAULT_ROT_VEL * DEGTORAD;
	 }
	else 
	 { rcclDefaultRotVel = FABS(rcclDefaultRotVel);
	 }

	simflag = 0;
	if (rcclGetOptionsSet() & RCCL_SIMULATE)
	 { /* simulation option has been set by the programmer. */

	   if (rcclGetOptions(RCCL_SIMULATE))
	    { simflag = 1;
	      if (rcclGetSimulator() == NULL)
	       { if ((status = getparString (&simName,"simulate",par)) == -1)
		  { clearErrors();
		  }
		 else if (status == 1 && simName[0] != '\0')
		  { rcclSetSimulator (simName);
		  }
	       }
	    }
	 }
	else
	 { /* simulation option has NOT been set by programmer; use whatever
	      is set in the .rciparams file. */

	   if ((status = getparInteger (&simflag, "simulate", par)) == -1)
	    { clearErrors();
	      if ((status = getparString (&simName, "simulate", par)) == -1)
	       { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "simulate");
		 return (-1);
	       }
	      if (status == 1)
	       { if (simName[0] != '\0')
		  { simflag = 1;
		    rcclSetOptions (RCCL_SIMULATE);
		    rcclSetSimulator (simName);
		  }
		 else
		  { rcclClearOptions (RCCL_SIMULATE);
		  }
	       }
	    }
	   else if (status == 1)
	    { if (simflag)
	       { rcclSetOptions (RCCL_SIMULATE);
	       }
	      else
	       { rcclClearOptions (RCCL_SIMULATE);
	       }
	    }
	 }
	if (simflag)
	 { if (rcclGetSimulator())
	    { printf ("simulation on %s\n", rcclGetSimulator());
	    }
	   else
	    { printf ("default simulation\n");
	    }
	 }

	/* timeout parameter */

	if (getparInteger (&count, "timeout", par) == 1)
	 { if (count < 1)
	    { errorMsg (ERange, rcclBadParamMsg, fxnName, "timeout");
	      return (-1);
	    }
	   timeoutCount = count;
	 }

	/* get default scheduling discipline */

	if ((status = getparIdentifier (&schedStr, "scheduling", par)) == -1)
	 { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "scheduling");
	   return (-1);
	 }
	if (status == 1 && scheduleDisp == UNDEF)
	 { if (strcmp (schedStr, "ON_TRIGGER") == 0)
	    { scheduleDisp = ON_TRIGGER;
	    }
	   else if (strcmp (schedStr, "ON_CLOCK") == 0)
	    { scheduleDisp = ON_CLOCK;
	    }
	   else if (strcmp (schedStr, "ON_FUNCTION") == 0)
	    { scheduleDisp = ON_FUNCTION;
	    }
	   else
	    { errorMsg (ENotDefined, rcclBadParamMsg, fxnName, "scheduling");
	      return (-1);
	    }
	 }

	if (getparInteger (&leavePowerOn, "leavePowerOn", par) == 1)
	 { if (leavePowerOn)
	    { rcclSetOptions (RCCL_LEAVE_POWER_ON);
	    }
	   else
	    { rcclClearOptions (RCCL_LEAVE_POWER_ON);
	    }
	 }

	serviceDelay = rcclGetMotionDelay();
	if ((rcclGmem = (RCCL_GMEM*)rciGlobalMemory (
			  "RCCL_GMEM", sizeof(RCCL_GMEM), ~0, 0)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   return (-1);
	 }
	rcclInitOptions ();
	rcclGmem->serviceDelay = serviceDelay;
	rcclSetSysTime (0.0);
	if ((rcclGmem->manipPool[ARBITER_CPU] = (MANIP*)rciGlobalMemory (
		   NULL, sizeof(MANIP)*MAX_MANIPS, ~0, ARBITER_CPU)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   return (-1);
	 }
	init_syncro();
	rcclGmem->interval = interval;

	envInitDone = 1;
	return (1);
}

static cpuSelect (cpuMask)
unsigned long cpuMask;
{
	/* Picks the CPU on which we will run the trajectory task */

	unsigned long permitted;
	unsigned long mask;
	int usage;
	int cpu;

	/* cpu selection is done as follows:

	   The 'mask' and the available cpus are 'anded' to provide
	   a permisible set of cpus.

	   Within this set, a cpu is selected that is (1) free to
	   the system, and (2) has the lowest number of tasks
	   running on it. The routine returns -1 if a
	   cpu can't be found.

	   The 'simulate' option forces the cpu to be 0.
	*/

	if (simflag)
	 { return (0);
	 }
	else
	 { if (cpuMask)
	    { permitted = availableCpus & cpuMask;
	    }
	   else
	    { permitted = availableCpus;
	    }
	 }
	for (usage = 0; usage <= manipsAllocated; usage++)
	 { for (cpu = MAX_CPUS-1; cpu >= 0; cpu--)
	    { if ((1<<cpu) & permitted)
	       { if (cpuUsage[cpu] <= usage)
		  { if (rciAllocCpus (1<<cpu) == (1<<cpu))
		     { return (cpu);
		     }
		  }
	       }
	    }
	 }	    
	return (-1);
}

extern setpTaskInit();
extern setpManipInit();
extern setpManipRemove();

static setpointTaskCreate (cpu)
int cpu;
{
	/* Called the first time we create a manipulator on 
	   a particular CPU. It creates a trajectory task on 
	   that CPU which will service all manipulators assigned
	   to that CPU now and later.

	   The trajectory task is an RCI control task.
	*/

	RCI_DESC *td;
	char taskname[32];
	char *fxnName = "setpointTaskCreate()";

	extern int setpStartup(); /* trajectory task functions */
	extern int setpCtrl();
	extern int setpRelease();

	sprintf (taskname, "RCCL%d", cpu);
	if ((td = rciCreate (taskname, (1<<cpu),simflag)) == NULL)
	 { return (-1);
	 }
	if (simflag)
	 { if (simScheduling (td) < 0)
	    { return (-1);
	    }
	 }
	else
	 { if (rciScheduling (td, scheduleDisp, 
				  cpu == ARBITER_CPU ? pollingDev : UNDEF) < 0)
	    { return (-1);
	    }
	 }
	if (rciRate (td, rcclGmem->interval, 0) < 0)
	 { return (-1);
	 }
	if (rciTimeout (td, timeoutCount, UNDEF, UNDEF) < 0)
	 { return (-1);
	 }
	if (rciControlFxns (td, setpStartup, setpCtrl, setpRelease, 0) < 0)
	 { return (-1);
	 }
	if ((sharedMem[cpu] = (RCCL_SMEM*)rciSharedMemory (
			  "RCCL_SMEM", sizeof(RCCL_SMEM), td)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   return (-1);
	 }
	if (cpu != ARBITER_CPU)
	 { if ((rcclGmem->manipPool[cpu] = (MANIP*)rciGlobalMemory (
		     NULL, sizeof(MANIP)*MAX_MANIPS, ~0, cpu)) == NULL)
	    { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	      return (-1);
	    }
	 }
	newManipTable(cpu);

	if (rciExecute (td, setpTaskInit, 0, (int*)NULL) < 0)
	 { return (-1);
	 }

	rcclTd[cpu] = td;
	taskMask |= TASKMASK(td);
	return (1);
}

static MANIP_DESC *allocManipMemory (cpu)
{
	/* Allocate the various pieces of memory we need for a
	   single manipulator.
	*/

	char *fxnName = "allocManipMemory()";

	MANIP_DESC *md;
	char *mem;
	int size;
	RCI_DESC *td = rcclTd[cpu];

	/* Memory local to the planning task: */

	if ((md = ALLOC(MANIP_DESC)) == NULL ||
	    (md->mvb = ALLOC(MOTION_VALUE_BLOCK)) == NULL ||
	    (md->mr = ALLOC(MOTION_REQUEST)) == NULL)
	 { errorMsg (EMemory, rcclAllocMemMsg, fxnName);
	   return (NULL);
	 }

	/* Setup the global memory sections */

	size =   ALIGN_SIZE(sizeof(MOTION_CONTROL_BLOCK)) 
	       + ALIGN_SIZE(sizeof(MOTION_QUEUE));
	if ((mem = (char*)rciGlobalMemory (NULL, size, ~0, cpu)) == NULL)
	 { return (NULL);
	 }
	ASSIGN_MEM (md->mcb, MOTION_CONTROL_BLOCK*, 
			     sizeof(MOTION_CONTROL_BLOCK), mem);
	ASSIGN_MEM (md->mq, MOTION_QUEUE*, sizeof(MOTION_QUEUE), mem);

	if ((md->mpb = (MOTION_PARAMETER_BLOCK*)rciLocalMemory (NULL,
		   sizeof(MOTION_PARAMETER_BLOCK), td)) == NULL)
	 { return (NULL);
	 }
	manipsAllocated++;
	return (md);
}

static setpointManipAttach (md, name, cpu)
MANIP_DESC *md;
char *name;
int cpu;
{
	/* This routine arranges and initializes the memory structures
	   required to control a partiular manipulator. It also informs
	   the trajectory task about the manipulator.

	   The memory is already assumed to be allocated, and pointers
	   to the required memory areas contained in the MANIP_DESC,
	   except for the shared objects hanging of the of the MANIP
	   structure (like 't6' and 'jvel'). These are allocated here
	   and freed when the manipulator is deleted.
	*/

	char *fxnName = "setpointManipAttach()";

	RCCL_SMEM *smem = sharedMem[cpu];
	MANIP *mnp;
	int rbtNum;
	int manipNum;
	int errorExit;
	int k;

	/* Allocate a number for the manipulator */

	manipNum = UNDEF;
	for (k=0; k<MAX_MANIPS; k++)
	 { if (!smem->manipTable[k].allocated)
	    { manipNum = k;
	      break;
	    }
	 }
	if (manipNum == UNDEF)
	 { errorMsg (ETooMany, rcclTooManyMsg, fxnName, "manips", MAX_MANIPS);
	   return (-1);
	 }
	
	mnp = md->mnp = &rcclGmem->manipPool[ARBITER_CPU][manipNum];

	bzero ((void*)mnp, sizeof(MANIP));
	bzero ((void*)md->mcb, sizeof(MOTION_CONTROL_BLOCK));

	/* Allocate and initialize the required objects attached to
	  the 'mnp' structure. */

	errorExit = rcclGetOptions (RCCL_ERROR_EXIT);
	rcclClearOptions (RCCL_ERROR_EXIT);

	if ((mnp->j6 = ALLOC_OBJECT (NULL, JNTS, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	zeroJnts (mnp->j6);		/* reset by setpoint at startup */
	if ((mnp->j6o = ALLOC_OBJECT (NULL, JNTS, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	zeroJnts (mnp->j6o);		/* reset by setpoint at startup */
	if ((mnp->jbias = ALLOC_OBJECT (NULL, JNTS, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	zeroJnts (mnp->jbias);		/* reset by setpoint at startup */
	if ((mnp->jvel = ALLOC_OBJECT (NULL, JNTS, cpu))== NULL)
	 { goto allocObjErr;
	 }
	zeroJnts (mnp->jvel);
	if ((mnp->t6 = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	setTransVarb (mnp->t6);
	identTrsf (mnp->t6);		/* reset by setpoint at startup */

	if ((mnp->t6o = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	setTransVarb (mnp->t6o);
	identTrsf (mnp->t6o);		/* reset by setpoint at startup */

	if ((mnp->here = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	setTransVarb (mnp->here);
	identTrsf (mnp->here);		/* reset by setpoint at startup */

	if ((mnp->tool = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	setTransVarb (mnp->tool);
	identTrsf (mnp->tool);

	if ((mnp->rest = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }
	if ((mnp->lastTC = allocTrans (NULL, cpu)) == NULL)
	 { goto allocObjErr;
	 }

	setTransVarb (mnp->lastTC);
	identTrsf (mnp->lastTC);

	/* Make the built in equations 'park' and 'last' */

	if ((mnp->park = makePosition ((char*)0,
				       T6, EQ, mnp->rest, TL, T6)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   freeManip (mnp, cpu);
	   rcclSetOptions (errorExit);
	   return (-1);
	 }
	if ((mnp->last = makePosition ((char*)0,
				       T6, EQ, mnp->lastTC, TL, T6)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   freeManip (mnp, cpu);
	   rcclSetOptions (errorExit);
	   return (-1);
	 }

	rcclSetOptions (errorExit);

	/* now initialize the various structures */

	STRNCPY(mnp->name, name, RCI_NAMESIZE);
	mnp->objectFlag = 0;	
	if (!find_robot(name))
	 { mnp->objectFlag = 1;
	 }
	
	/* Attach the system memory structures to mnp: */

	mnp->mq = (void*)md->mq;
	mnp->mcb = (void*)md->mcb;
	init_queue (md->mq);

	mnp->mvb = (void*)md->mvb;
	mnp->mr = (void*)md->mr;
	
	/* should move these to a routine, but this will do for now */
	md->mcb->motionQueueSize = DEFAULT_QUEUE_SIZE;
/*	md->mcb->goalPosBuf = md->mpb->goalPosBuf;	*/
	
	/* Attach the required robot to the trajectory task */

	rbtNum = UNDEF;		/* in case we have object frame */
	if (simflag)
	 { if (!(mnp->objectFlag))
	    { if ((rbtNum = simAttachRobot (rcclTd[cpu], mnp->name,
					    rcclGetSimulator())) < 0)
	       { freeManip (mnp, cpu);
		 return (-1);
	       }
	    }
	 }
	else
	 { if (!(mnp->objectFlag))
	    { if ((rbtNum = rciAttachRobot (rcclTd[cpu], mnp->name, NULL)) < 0)
	       { freeManip(mnp, cpu);
		 return (-1);
	       }
	    }
	 }

	mnp->rbtNum = rbtNum;
	mnp->taskNum = TASKNUM(rcclTd[cpu]);
	mnp->cpu = cpu;
	mnp->num = manipNum;

	initMcb (mnp);	/* have to do this AFTER we have attached robot */
	motionRequestInit (mnp);

	if (rbtNum != UNDEF)
	 { /* evalutate park angles if we have a real robot */
	
	   RCI_RBT *rbt = MANIP_ROBOT(mnp);
	   FWD_KINEMATICS (mnp->rest, NULL, rbt->jls->parkAngles, NULL);

	   numRobots++;

	   /* assign an interrupt device if this is the first robot
	      on the arbiter task */

	   if (cpu == ARBITER_CPU)
	    { if ((TASKNUMROBOTS(rcclTd[0]) == 1) && !simflag)
	       { pollingDev = rbt->idev;
		 if (rciScheduling (rcclTd[0], scheduleDisp, pollingDev)<0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    freeManip(mnp, cpu);
		    return (-1);
		  }
	       }
	    }
	   initJntsByManip (mnp->j6, mnp);
	   initJntsByManip (mnp->j6o, mnp);
	   initJntsByManip (mnp->jbias, mnp);
	   initJntsByManip (mnp->jvel, mnp);
	   
	   mnp->rbt = MANIP_ROBOT(mnp);
	   mnp->how = MANIP_ROBOT(mnp)->how;
	   mnp->jls = MANIP_ROBOT(mnp)->jls;
	   mnp->kyn = MANIP_ROBOT(mnp)->kyn;
	   mnp->var = MANIP_ROBOT(mnp)->var;
	 }
	else
	 { identTrsf (mnp->rest);
	   mnp->rbt = NULL;
	   mnp->how = NULL;
	   mnp->jls = NULL;
	   mnp->kyn = NULL;
	   mnp->var = NULL;
	 }
	   
	/* load the new manip information into the task structure */

	addManipTableEntry (manipNum, cpu, (void*)md->mpb, (void*)md->mnp);
	smem->mpb[manipNum] = md->mpb;

	/* lastly, tell each task about the new MANIP structure */

	for (k=0; k<MAX_CPUS; k++)
	 { if (rcclTd[k])
	    { if (rciExecute (rcclTd[k], setpManipInit, manipNum, NULL) < 0)
	       { errorMsg (EInternal, rcclNullMsg, fxnName);
		 return (-1);
	       }
	    }
	 }

	return (1);

allocObjErr:
	rcclSetOptions (errorExit);
	errorMsg (EMemory, rcclAllocMemMsg, fxnName);
	freeManip(mnp, cpu);
	return (-1);
}

static setpointManipDetach (md)
MANIP_DESC *md;
{
	/* This routine is, predictably, the inverse of 
	   setpointManipAttach(). It frees the objects attached 
	   to the manip structure, and tells the trajectory generator
	   to remove the manipulator from its list of servicable
	   manipulators.
	 */

	char *fxnName = "setpointManipDetach()";
	MANIP *mnp = md->mnp;
	RCCL_SMEM *smem = sharedMem[mnp->cpu];
	int code = 1;
	int k;

	/* Tell each task to remove the manipulator */

	for (k=0; k<MAX_CPUS; k++)
	 { if (rcclTd[k])
	    { if (rciExecute (rcclTd[k], setpManipRemove, mnp->num, NULL) < 0)
	       { errorMsg (EInternal, rcclNullMsg, fxnName);
		 return (-1);
	       }
	    }
	 }

	if (MANIP_ROBOT(mnp))
	 { numRobots--;

	    /* if the robot is attached to the arbiter, and it is the
	       last robot so attached, then set the polling interrupt
	       to UNDEF */

	   if (mnp->cpu == ARBITER_CPU)
	    { if (TASKNUMROBOTS(rcclTd[0]) == 1 && !simflag)
	       { pollingDev = UNDEF;
		 if (rciScheduling (rcclTd[0], scheduleDisp, UNDEF)<0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    code = -1;
		  }
	       }
	    }

	   if (rciDetachRobot (rcclTd[mnp->cpu], mnp->rbtNum) < 0)
	    { errorMsg (EInternal, rcclNullMsg, fxnName);
	      code = -1;
	    }
	 }

	smem->manips[mnp->num] = NULL;
	deleteManipTableEntry (mnp->num);
	freeManip (mnp, mnp->cpu);
	return(code);
}

static freeManip (mnp, cpu)
MANIP *mnp;
int cpu;
{
	/* free all of the objects attached to a MANIP structure */

	if (mnp->j6)
	 { freeMem (mnp->j6);
	 }
	if (mnp->j6o)
	 { freeMem (mnp->j6o);
	 }
	if (mnp->jbias)
	 { freeMem (mnp->jbias);
	 }
	if (mnp->jvel)
	 { freeMem (mnp->jvel);
	 }
	if (mnp->t6)
	 { freeTrans (mnp->t6);
	 }
	if (mnp->t6o)
	 { freeTrans (mnp->t6o);
	 }
	if (mnp->here)
	 { freeTrans (mnp->here);
	 }
	if (mnp->tool)
	 { freeTrans (mnp->tool);
	 }
	if (mnp->rest)
	 { freeTrans (mnp->rest);
	 }
	if (mnp->lastTC)
	 { freeTrans (mnp->lastTC);
	 }
	if (mnp->park)
	 { freePosition (mnp->park);
	 }
	if (mnp->last)
	 { freePosition (mnp->last);
	 }
	return (1);
}

static addManipTableEntry (num, cpu, local, shared)
int num, cpu;
void *local;
void *shared;
{
	/* Add information about the MANIP into the shared memory area
	   for each control task.
	 */
	int k;
	MANIP_RECORD *entry;

	for (k=0; k<MAX_CPUS; k++)
	 { if (sharedMem[k])
	    { entry = &sharedMem[k]->manipTable[num];
	      entry->cpu = cpu;
	      entry->allocated = 1;
	      if (cpu == k)
	       { entry->localMem = local;
		 entry->sharedMem = shared;
	       }
	    }
	 }
}

static deleteManipTableEntry (num)
int num;
{
	/* Remove the manipulator from the shared memeory table 
	   associated with every trajectory task.
	 */
	int k;

	for (k=0; k<MAX_CPUS; k++)
	 { if (sharedMem[k])
	    { sharedMem[k]->manipTable[num].allocated = 0;
	    }
	 }
}

static newManipTable (cpu)
int cpu;
{
	/* Set up the manip table in shared memory between the planning
	   level and a newly created trajectory task.
	 */

	MANIP_RECORD *table1 = NULL;
	MANIP_RECORD *table2;
	int k;

	for (k=0; k<MAX_CPUS; k++)
	 { if (sharedMem[k] && k != cpu)
	    { table1 = sharedMem[k]->manipTable;
	      break;
	    }
	 }
	table2 = sharedMem[cpu]->manipTable;
	if (table1)
	 { for (k=0; k<MAX_MANIPS; k++)
	    { table2[k] = table1[k];
	    }
	 }
	else
	 { for (k=0; k<MAX_MANIPS; k++)
	    { table2[k].allocated = 0;
	    }
	 }
}

rcclSetInterval (interval) /*::*/
int interval;
{
	/* Basic primitive to change the trajectory task sample interval.
	   Must be called when control is not running.
	 */

	char *fxnName = "rcclSetInterval()";
	unsigned long active;
	int k;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();

	if (!envInitDone)
	 { doEnvInit();
	 }

	if (interval < 1)
	 { errorMsg (ERange, rcclBadArgMsg, fxnName, interval);
	   RETURN_ERROR (-1);
	 }

	/* make sure all tasks are released */

	if (active = tasksAreActive())
	 { errorMsg (ETaskActive, rcclIsActiveMsg, fxnName, active);
	   RETURN_ERROR (-1);
	 }
	for (k = 0; k<MAX_CPUS; k++)
	 { if (rcclTd[k])
	    { if (rciRate (rcclTd[k], interval, 0) < 0)
	       { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
		 RETURN_ERROR (-1);
	       }
	      if (rciTimeout (rcclTd[k], timeoutCount, UNDEF, UNDEF) < 0)
	       { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
		 RETURN_ERROR (-1);
	       }
	    }
	 }
	rcclGmem->interval = interval;
	RETURN_NORMAL (1);
}

rcclStart()
{
	/* Basic RCCL primitive to start the trajectory generator */

	char *fxnName = "rcclStart()";
	extern int rcclMotionNumber;

	MANIP_DESC *md;
	RCI_RBT *rbt;
	int k;
	int powerOnMsg = 0;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }

	/* If we recently released, then wait a bit before trying
	   to connect again. This doesn't really belong here ...
	   we should implement a retry in rciStart(). But this will
	   have to do for now. */

	if (releaseTime)
	 { struct timeval tv;
	   double tdiff;

	   getSystemTime(&tv);
	   tdiff = timevalToDouble(tv) - (releaseTime + 0.2);
	   if (tdiff < 0)
	    { nap (-tdiff);
	    }
	 }

	MASK_RCI_AUTO_ABORT();

	for (md = manipList->next; md != manipList; md = md->next)
	 { init_queue (md->mnp->mq);
	   md->mcb->headMotionNumber = 0; /* XXX */
	   md->mcb->tailMotionNumber = 0;	   
	 }
	init_syncro();
	rcclMotionNumber = 0;
	rcclGmem->motionFlagNumber = 0;

	if (rciStart (taskMask) < 0)
	 { errorMsg (getErrorCode(0), rcclTaskSetupMsg, fxnName);
	   RETURN_ERROR (-1);
	 }

	/* now wait for the tasks to start, and for power to come on */

	for (k=0; k<MAX_CPUS; k++)
	 { if (rcclTd[k])
	    { rciBlock(rcclTd[k]);
	      rciBlock(rcclTd[k]);
	      break;
	    }
	 }
	for (md = manipList->next; md != manipList; md = md->next)
	 { if (MANIP_ROBOT(md->mnp) && !(MANIP_ROBOT(md->mnp)->how->power))
	    { if (!powerOnMsg)
	       { printf ("Turn on arm power\n");
		 powerOnMsg = 1;
	       }
	    /* MS Expand waitfor macro for debugging */
	    /* waitfor (MANIP_ROBOT(md->mnp)->how->power); */

	    do { if (rciTaskId == 0) rcclBlock(); } while(!(
			    MANIP_ROBOT(md->mnp)->how->power));
	    }
	 }
}

rcclRelease(powerOff)
int powerOff;
{
	/* Basic RCCL primitive to stop the trajectory generator */

	MANIP_DESC *md;
	RCI_RBT *rbt;
	char *fxnName = "rcclRelease()";
	struct timeval tv;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if ((!powerOff || rcclGetOptions(RCCL_LEAVE_POWER_ON)) 
	     && tasksAreActive())
	 { for (md = manipList->next; md != manipList; md = md->next)
	    { if (rbt = MANIP_ROBOT(md->mnp))
	       { SET_POWER_ON_RELEASE(rbt,YES);
	       }
	    }
	 }
	rcclBlock();	/* wait to see the SET_POWER_ON_RELEASE */
	
	if (rciRelease (taskMask) < 0)
	 { RETURN_ERROR (-1);
	 }
	dereferenceAllTransforms();

	getSystemTime(&tv);
	releaseTime = timevalToDouble(tv);	

	RETURN_NORMAL(1)
}

rcclTailMotionNumber()
{
	/* This routine returns the highest motion number, below which
	   we know all other motions have been completed. The
	   algorithm works in this way:

	   if there are no manipulators with outstanding motions,
	    { n = motionNumber;
	    }
	   else
	    { from all manipulators with outstanding motions,
		 n = the lowest completed motion number.
	    }

	   This routine by the freeTrans() and allocTrans() code to see
	   when a transform is no longer being referenced by the control
	   level and may be safely freed.
	*/

	extern int rcclMotionNumber;

	MANIP_DESC *md;
	MOTION_CONTROL_BLOCK *mcb;
	int lowest;	

	INIT_CHECK();

	lowest = UNDEF;
	if (manipList->next == NULL)	/* nothing set up yet */
	 { return (rcclMotionNumber);
	 }
	for (md = manipList->next; md != manipList; md = md->next)
	 { mcb = md->mcb;
	   if (mcb->headMotionNumber > mcb->tailMotionNumber)
	    { if (lowest == UNDEF || mcb->tailMotionNumber < lowest)
	       { lowest = mcb->tailMotionNumber;
	       }
	    }
	 }
	if (lowest == UNDEF)
	 { return (rcclMotionNumber);
	 }
	else
	 { return (lowest);
	 }
}

rcclBlock ()
{
	/* Block for one entire trajectory cycle */
	 
	char *fxnName = "rcclBlock()";
	int k;

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	for (k=0; k<MAX_CPUS; k++)
	 { if (rcclTd[k])
	    { return (rciBlock(rcclTd[k]));
	      break;
	    }
	 }
	return (0);
}

rcclAbortReset()
{
	/* should be called from any user defined rci abort handler */

	char *fxnName = "rcclAbortReset()";
	struct timeval tv;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	getSystemTime(&tv);
	releaseTime = timevalToDouble(tv);	
	dereferenceAllTransforms();
	return (1);
}

initMcb (mnp)
MANIP *mnp;
{
	/* set initial settings in the motion control block.

	   (things are kind of quite in this routine right now!) */

	char *fxnName = "initMcb()";
	MOTION_CONTROL_BLOCK *mcb = (MOTION_CONTROL_BLOCK*)mnp->mcb;

	mcb->mode 	= rcclDefaultModes();
	mcb->activeMode = mcb->mode;

}

rcclScheduling(disp)
int disp;
{
	/* preset the rccl scheduling discipline. Must be called
	   before first call to rcclCreate() */

	char *fxnName = "rcclScheduling()";

	INIT_CHECK();

	switch (disp)
	 { default:
	    { errorMsg (EBadArg, rcclBadArgMsg, fxnName, disp);
	      RETURN_ERROR(-1);
	      break;
	    }
	   case ON_FUNCTION:
	   case ON_TRIGGER:
	   case ON_CLOCK:
	    { scheduleDisp = disp;
	      break;
	    }
	 }
	RETURN_NORMAL(1);	
}
