/*-=-=-=-=-=-=-=-=-=-=-=- 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:	transSetup
 *
 * DESCRIPTION:	Planning level routines which allocate RCCL transform
 *		structures and control their binding to functions.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------

	The transform package is implemented using the following methods.

	Each transform is a piece of global shared memory that can be 
	accessed from any part of an RCCL program. Valid addresses for a
	particular transform can be obtained by name ('getTransByName()'), or by
	using the planning task's address ('getTransByAddr()'). Atomic reads
	and writes on transforms can be performed using 'readTrans()' and
	'writeTrans()'. 

	The value of a transform can be set by the user program. When a
	transform is found in the target position of a motion request, the
	system assumes that the current transform value should be the one
	usedfor the entire motion request. It makes a private copy of the
	transform which gets handed to the trajectory generator along with the
	motion request. The trajectory generator will refer to this local copy
	when performing the motion requests. In these cases, the transform is
	considered to be "constant". 

	If a transform is set to be "variable", then the trajectory
	generator will contantly update its local copy with the current value
	of the global (or parent) transform. The effect of changing a variable
	transform is tracked immediately by all motions which refer to it.

	The modification of a variable transform may be done by binding it
	to a function. This function is then evaluated by the trajectory
	generator at the control level, once per control cycle. This function
	evaluation may be requested in two ways: permanently ('transEval()'),
	or on a per-motion basis, where the evaluation only occurs when the
	transform is refered to by a motion request for a particular
	manipulator ('transMotionEval()'). In both these case, the trajectory
	generator treats the function as variable when refering to it, and
	copies its global value into the local trajectory copy once per cycle.
	
	When calling the evaluation function for a transform, the trajectory
	task also uses a local copy of the transform, which is later
	atomically copied into the global transform (in the case of per-motion
	function binding, the local target position copy and the local
	evaluation copy happen to be the same).

	       global (parent)             local copy for motion
	         transform                    request target
		 __________			 __________
		 |        |			 |	  |
		 |        |			 |	  |
		 |        |----readTrans()------>|        |
		 |        |			 |	  |
		 |________|			 |________|
		      ^
		      |
		  writeTrans()
		      |
		 _____|____
		 |        |		     
		 |        | local copy for
		 |        | function evaluation
		 |        |		     
		 |________|		     
		
 --------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/rciTask.h"
#include "../h/allocMem.h"
#include "../h/errorCodes.h"
#include "../h/rciMacros.h"
#include "../h/rcclDefs.h"
#include "../h/mutex.h"

#include "../rccl/transform.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"

#include "../rci/link.h"
#include "../h/rciConfig.h"
#include "../rci/hashTable.h"

static TRSF_DESC _freeWaitList; /* list of transforms waiting to freed */
static TRSF_DESC *freeWaitList;	
static TRSF_DESC _transformList;/* list of all allocated transforms */
static TRSF_DESC *transformList;

extern RCI_DESC *rcclTd[];	/* RCCL task descriptors for each CPU */

extern int transDebug;		/* flag for debugging messages */

/* For use by testing programs */
#define TRTESTSIZE	2
int trtestLinkBp = 0;
TRSF *trtestLinkBuf[TRTESTSIZE];
int trtestUnlinkBp = 0;
TRSF *trtestUnlinkBuf[TRTESTSIZE];

static initTransDesc();
static checkFreeList();
static doSetTrans();

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	/* Called once at the beginning to initialize the list
	   structures.
	 */
	initDone = 1;
	freeWaitList = &_freeWaitList;
	freeWaitList->next = freeWaitList->prev = freeWaitList;
	transformList = &_transformList;
	transformList->next = transformList->prev = transformList;

	trtestLinkBp = 0;
	trtestUnlinkBp = 0;
	requestFlagInit (&initDone);
}

TRSF *allocTrans (name, pool)
const char *name;
int pool;
{
	/* RCCL primitive to allocate a transform from the specified
	   memory pool.
	 */
	char *fxnName = "allocTrans()";
	TRSF *tr;

	INIT_CHECK();

	if (!planningLevelCheck (fxnName))
	 { return (NULL);
	 }
	if ((tr = doAllocTrans (name, pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	else
	 { RETURN_NORMAL (tr);
	 }
}

TRSF *doAllocTrans (name, pool)
const char *name;
int pool;
{
	/* Internal routine that actually does the work of allocating
	   a transform.
	 */

	char *fxnName = "doAllocTrans()";

	TRSF_DESC *tr;

	INIT_CHECK();

	if (pool == UNDEF)
	 { pool = 0;	/* use memory on CPU 0 by default */
	 }
	
	/* check the free wait list for transforms we can free */

	checkFreeList();


#if 0	/* don't free the transform if already allocated */
	if (name != NULL)
	 { for (tr = transformList->next; tr != transformList; tr = tr->next)
	    { if (tr->name && strcmp (tr->name, name) == 0)
	       { freeTrans (tr);
	       }
	    }
	 }
#endif

	/* allocate the transform as an RCI object */

	if ((tr = (TRSF_DESC*)allocMemId (name, TRSF_HASH_ID, 
					  sizeof(TRSF_DESC), pool)) == NULL)
	 { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	   return (NULL);
	 }

	/* initialize some values and link it into the system list */

	initTransDesc (tr);
	
	/* If 'pool' is a CPU number, record this as the prefered
	   CPU to use when looking for a trajectory task to evaluate
	   a permanent function binding.
	 */
	if (0 <= pool < MAX_CPUS)
	 { tr->cpu = pool;
	 }
	tr->name = getMemName (tr);
	LINK (tr, transformList);

	return (&tr->trsf);
}

static initTransDesc(tr)
TRSF_DESC *tr;
{
	/* Intializes the value of an RCCL transform */

	tr->next = NULL;
	tr->prev = NULL;
	tr->source = tr;
	tr->parent = tr;
	tr->fxn = constTrsf;
	tr->arg = 0;
	tr->mark = 0;
	tr->manipOwner = 0;
	tr->taskOwner = 0;
	tr->cpu = UNDEF;
	tr->timestamp = 0;
	tr->magic = TRSF_MAGIC;
	MUTEX_CLEAR (&tr->mutex);
	identTrsf (&tr->trsf);
}


freeTrans (t)
TRSF *t;
{
	/* RCCL primitive to free an allocated transform */

	char *fxnName = "freeTrans()";

	TRSF_DESC *tr = TRSF_GETDESC(t);

	INIT_CHECK();

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

	/* check the free wait list for transforms we can free */

	checkFreeList(); 

	if (tr->magic != TRSF_MAGIC)	/* valid Xform pointer? */
	 { RETURN_NORMAL (0);
	 }

	/* If the transform is bound to a function, unbind it */

	if (tr->taskOwner)
	 { if (taskDropTransform (tr) < 0)
	    { errorMsg (EInternal, rcclNullMsg, fxnName);
	      RETURN_ERROR (-1);
	    }
	 }
	else if (tr->manipOwner)
	 { if (manipDropTransform (tr) < 0)
	    { errorMsg (EInternal, rcclNullMsg, fxnName);
	      RETURN_ERROR(-1);
	    }
	 }

	tr->magic = 0;		/* clear magic number */
	UNLINK (tr);		/* remove from transformList */

	/* If the transform is currently being referenced by 
	   the trajectory generator, it is very important not
	   that we don't complete the free until the trajectory
	   generator is through with it. Otherwise, we could
	   call allocTrans() again, get the same memory back,
	   set it, and wonder why the robot decided to fly across
	   the room. So it it is being referenced by a trajectory
	   generator, we put the transform on a wait list and
	   physically free it when the trajectory task is finished
	   with it.
	 */
	if (tr->mark > rcclTailMotionNumber())
	 { if (transDebug)
	    { printf ("link '%s' onto free wait list\n", 
		      tr->name);
	    }
	   if (trtestLinkBp < TRTESTSIZE)	/* for test program */
	    { trtestLinkBuf[trtestLinkBp++] = &tr->trsf;
	    }
	   LINK (tr, freeWaitList);
	 }
	else
	 { freeMemId (tr, TRSF_HASH_ID);
	 }
	RETURN_NORMAL (1);
}

static checkFreeList()
{
	/* This routine goes through and frees transforms for 
	   which freeTrans() was called but could not be freed 
	   because they were being referenced by the trajectory task.

	   The strategy for checking the transforms is as follows:

	   Every time we issue a move request, we mark all variable
	   transforms with the number of the motion reqest. Later,
	   if the transform is freed but its motion number is greater
	   than the number of the last motion serviced by the trajectory
	   generator, we put in on a wait list until this condition is true.
	
	   This routine checks the wait list.
	 */

	TRSF_DESC *tr;
	int n;

	if (freeWaitList->next != freeWaitList)
	 { 
	   /* first, find the lowest motion */

	   n = rcclTailMotionNumber();
	    
	   for (tr = freeWaitList->next; tr != freeWaitList; tr = tr->next)
	    { if (tr->mark <= n)
	       { if (transDebug)
		  { printf ("unlinking transform '%s'\n", tr->name);
		  }
		 if (trtestUnlinkBp < TRTESTSIZE)	/* for test program */
		  { trtestUnlinkBuf[trtestUnlinkBp++] = &tr->trsf;
		  }
		 UNLINK (tr);
		 freeMemId (tr, TRSF_HASH_ID);
	       }
	    }
	 }
}

setTransConst(t)
TRSF *t;
{
	/* RCCL primitive to define an allocated  transform to
	   be constant. All the work is done by the routine
	   'doSetTrans()'.
	 */

	char *fxnName = "setTransConst()";
	TRANS_BINDING binding;

	binding.code = TRANS_CONST;
	binding.fxn = NULLF;
	binding.arg = 0;
	binding.mnp = NULL;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if (doSetTrans(fxnName, t, &binding) < 0)
	 { RETURN_ERROR (-1);
	 }		
	RETURN_NORMAL (1);
}

setTransVarb(t)
TRSF *t;
{
	/* RCCL primitive to define an allocated transform to
	   be variable. All the work is done by the routine
	   'doSetTrans()'.
	 */

	char *fxnName = "setTransVarb()";
	TRANS_BINDING binding;

	binding.code = TRANS_VARB;
	binding.fxn = NULLF;
	binding.arg = 0;
	binding.mnp = NULL;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if (doSetTrans(fxnName, t, &binding) < 0)
	 { RETURN_ERROR (-1);
	 }		
	RETURN_NORMAL (1);
}

transEval(t, fxn, arg)
TRSF *t;
int (*fxn)();
int arg;
{
	/* RCCL primitive to attach an allocated transform to
	   an evaluation function. All the work is done by the routine
	   'doSetTrans()'.
	 */

	char *fxnName = "transEval()";
	TRANS_BINDING binding;

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

	binding.code = TRANS_EVAL;
	binding.fxn = fxn;
	binding.arg = arg;
	binding.mnp = NULL;

	if (doSetTrans(fxnName, t, &binding) < 0)
	 { RETURN_ERROR (-1);
	 }		
	RETURN_NORMAL (1);
}

transMotionEval(t, fxn, arg, mnp)
TRSF *t;
int (*fxn)();
int arg;
MANIP *mnp;
{
	/* RCCL primitive to attach an allocated transform to
	   an evaluation function called only when the transform
	   is part of the target specification for a motion to the
	   manipulator 'mnp'. All the work is done by the routine
	   'doSetTrans()'.
	 */

	char *fxnName = "transMotionEval()";
	TRANS_BINDING binding;

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

	binding.code = TRANS_MOTION_EVAL;
	binding.fxn = fxn;
	binding.arg = arg;
	binding.mnp = mnp;

	if (doSetTrans(fxnName, t, &binding) < 0)
	 { RETURN_ERROR (-1);
	 }		
	RETURN_NORMAL (1);
}

setTransBinding (t, binding)
TRSF *t;
TRANS_BINDING *binding;
{
	/* RCCL primitive to arbitrarily set the binding of
	   an allocated transform. Again, all the work is done
	   by 'doSetTrans()'.
	 */

	char *fxnName = "setTransBinding()";

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	MASK_RCI_AUTO_ABORT();
	if (doSetTrans(fxnName, t, binding) < 0)
	 { RETURN_ERROR (-1);
	 }		
	RETURN_NORMAL (1);
}

static doSetTrans(fxnName, t, binding)
char *fxnName;
TRSF *t;
TRANS_BINDING *binding;
{
	/* Routine that actually does the work involved in setting
	   transform bindings.
	 */

	TRSF_DESC *tr = (TRSF_DESC*)t;	/* pointer punning */

	if (tr->magic != TRSF_MAGIC)
	 { errorMsg (EDescriptor, rcclBadTrsfMsg, fxnName, t);
	   return (-1);
	 }

	switch (binding->code)
	 { case TRANS_CONST:
	    {
	      /* Drop any existing function binding */

	      if (tr->taskOwner)
	       { if (taskDropTransform (tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
	      else if (tr->manipOwner)
	       { if (manipDropTransform (tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
	      tr->fxn = constTrsf;
	      break;
	    }
	   case TRANS_VARB:
	    { 
	      /* Drop any existing function binding */

	      if (tr->taskOwner)
	       { if (taskDropTransform (tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
	      else if (tr->manipOwner)
	       { if (manipDropTransform(tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
	      tr->fxn = varbTrsf;
	      break;
	    }
	   case TRANS_EVAL:
	    { 
	      int taskCpu = UNDEF;
	      int cpu;

	      /* Drop any motion function binding */
      
	      if (tr->manipOwner)
	       { if (manipDropTransform(tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
		
	      /* Check so see if the function is loaded as part
		 of the control segment. */

	      if (!(validControlText(binding->fxn)))
	       { errorMsg (EBadArg, rcclBadFxnMsg, fxnName, binding->fxn);
		 return (-1);
	       }
		
	      /* Find a task to do the function evaluation. If the
		 transform is allocated out of memory on a CPU,
		 and there is a trajectory task executing on that 
		 CPU, use that task. Otherwise, pick a task at
		 random, trying the auxiliary CPUs first and finally
		 the arbiter.
	       */
	      if (tr->cpu != UNDEF && rcclTd[tr->cpu] != NULL)
	       { taskCpu = tr->cpu;
	       }
	      else
	       { for (cpu = 1; cpu < MAX_CPUS; cpu++)
		  { if (rcclTd[cpu] != NULL)
		     { taskCpu = cpu;
		       break;
		     }
		  }
		 if (taskCpu == UNDEF)
		  { if (rcclTd[0] != NULL)
		     { taskCpu = 0;
		     }
		    else
		     { errorMsg (ENotAvail, rcclNoTasksMsg, fxnName);
		       return (-1);
		     }
		  }
	       }

	      /* Tell the chosen task about the new transform it has
		 to evaluate */
	      if (taskPickupTransform (tr, binding->fxn,
					   binding->arg, taskCpu) < 0)
	       { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
		 return (-1);
	       }
	      break;
	    }
	   case TRANS_MOTION_EVAL:
	    { 
	      /* Drop any current function bindings */

	      if (tr->taskOwner)
	       { if (taskDropTransform (tr) < 0)
		  { errorMsg (EInternal, rcclNullMsg, fxnName);
		    return (-1);
		  }
	       }
	      else if (tr->manipOwner)
	       { if (tr->manipOwner != (int)binding->mnp)
		  { if (manipDropTransform(tr) < 0)
		     { errorMsg (EInternal, rcclNullMsg, fxnName);
		       return (-1);
		     }
		  }
	       }

	      /* Check so see if the function is loaded as part
		 of the control segment. */

	      if (!(validControlText(binding->fxn)))
	       { errorMsg (EBadArg, rcclBadFxnMsg, fxnName, binding->fxn);
		 return (-1);
	       }

	      /* Check that we have an RCCL task on the CPU with which
		 the MANIP is associated (I think this is a tautology,
		  but it doesn't hurt to check anyway).
	       */		 
	      if (rcclTd[binding->mnp->cpu] == NULL)
	       { errorMsg (EInternal, rcclNoTasksMsg, fxnName);
		 return (-1);
	       }

	      /* Tell the task that it has a new transform to evaluate
		 when found inside a motion target ... */

	      if (manipPickupTransform(tr, binding->fxn, 
					   binding->arg, binding->mnp) < 0)
	       { errorMsg (getErrorCode(0), rcclNullMsg, fxnName);
	       }
	      break;
	    }
	 }
	return (1);
}

#include "../rccl/setpoint.h"

extern RCCL_SMEM *sharedMem[];
extern transCtrl();

manipDropTransform (tr)
TRSF_DESC *tr;
{
	/* Tell a trajectory task to remove 'tr' from the list
	   of transforms which are evaluated when they are included in
	   the target specification of the manipulator 'mnp'.

	   To do this we just set up some arguments, and execut
	   the function 'transCtrl' inside the trajectory task.
	 */

	char *fxnName = "manipDropTransform()";
	MANIP *mnp = (MANIP *)tr->manipOwner;
	RCCL_SMEM *smem = sharedMem[mnp->cpu];
	int retval;

	smem->transPtr = tr;
	smem->manipNum = mnp->num;
	if (rciExecute (rcclTd[mnp->cpu], transCtrl,
			MANIP_TRANS_DROP, &retval) < 0)
	 { return (-1);
	 }
	if (retval < 0)
	 { errorMsg (EInternal, rcclTrsfCtrlMsg, fxnName);
	   return (-1);
	 }
	return (1);
}

manipPickupTransform (tr, fxn, arg, mnp)
TRSF_DESC *tr;
int (*fxn)();
int arg;
MANIP *mnp;
{
	/* Tell a trajectory task to place 'tr' on the list
	   of transforms which are evaluated when they are included in
	   the target specification of the manipulator 'mnp'.

	   To do this we just set up some arguments, and execut
	   the function 'transCtrl' inside the trajectory task.
	 */

	char *fxnName = "manipPickupTransform()";
	int retval;
	RCCL_SMEM *smem = sharedMem[mnp->cpu];

	smem->transPtr = tr;
	smem->transFxn = fxn;
	smem->transArg = arg;
	smem->manipNum = mnp->num;
	smem->man = mnp;
	if (rciExecute (rcclTd[mnp->cpu], transCtrl,
			MANIP_TRANS_PICKUP, &retval) < 0)
	 { return (-1);
	 }
	if (retval < 0)
	 { errorMsg (EInternal, rcclTrsfCtrlMsg, fxnName);
	 }
	return (1);
}

taskDropTransform (tr)
TRSF_DESC *tr;
{
	/* Tell a trajectory task to remove 'tr' from the list
	   of transforms being constantly evaluated.

	   To do this we just set up some arguments, and execut
	   the function 'transCtrl' inside the trajectory task.
	 */

	char *fxnName = "taskDropTransform()";
	int cpu = TASKCPU(findRciTask(tr->taskOwner));
	RCCL_SMEM *smem = sharedMem[cpu];
	int retval;

	smem->transPtr = tr;
	if (rciExecute (rcclTd[cpu], transCtrl,
			TASK_TRANS_DROP, &retval) < 0)
	 { return (-1);
	 }
	if (retval < 0)
	 { errorMsg (EInternal, rcclTrsfCtrlMsg, fxnName);
	 }
	return (1);
}

taskPickupTransform (tr, fxn, arg, cpu)
TRSF_DESC *tr;
int (*fxn)();
int arg;
int cpu;
{
	/* Tell a trajectory task to add 'tr' to the list
	   of transforms being constantly evaluated.

	   To do this we just set up some arguments, and execut
	   the function 'transCtrl' inside the trajectory task.
	 */

	char *fxnName = "taskPickupTransform()";
	int retval;
	RCCL_SMEM *smem = sharedMem[cpu];

	INIT_CHECK();

	smem->transPtr = tr;
	smem->transFxn = fxn;
	smem->transArg = arg;
	if (rciExecute (rcclTd[cpu], transCtrl,
			TASK_TRANS_PICKUP, &retval) < 0)
	 { return (-1);
	 }
	if (retval < 0)
	 { errorMsg (EInternal, rcclTrsfCtrlMsg, fxnName);
	 }
	return (1);
}

referenceTransforms (p, number)
POS *p;
int number;
{
	/* Called by motionRequest() to mark all variable tansforms with 
	   the indicated motion number */

	register POS_TERM *t;
	register TRSF_DESC *tr;
	for (t = p->t6Term->prev; t != p->t6Term; t = t->prev)
	 { tr = TRSF_GETDESC(t->trsf);
	   if (tr->fxn != constTrsf)
	    { tr->mark = number;
	    }
	 }
}

dereferenceAllTransforms ()
{
	/* Called by rcclRelease() to clear the motion number reference
	   on all transforms and free any which are waiting to be freed.
	 */

	register TRSF_DESC *tr;

	INIT_CHECK();
	
	for (tr = transformList->next; tr != transformList; tr = tr->next)
	 { tr->mark = 0;
	 }
	for (tr = freeWaitList->next; tr != freeWaitList; )
	 { TRSF_DESC *temp = tr->next;
	   freeMemId (tr, TRSF_HASH_ID);
	   tr = temp;
	 }
}
