/*-=-=-=-=-=-=-=-=-=-=-=- 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:	transControl
 *
 * DESCRIPTION:	Control level code for managing RCCL transforms:
 *		accessing, seting their function bindings, and calling
 *		the evaluation functions.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

/* Set true if you want transform functions called with the old
   argument style. */

int oldTfxnArgs = 0;

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

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

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

extern int rciCpu; /* CPU task is running on */

/* Should make this these things dynamically allocated ... someday! */
#define MAX_TRSF_EVAL	32
TRSF_DESC trsfPool[MAX_TRSF_EVAL];
static TRSF_DESC *taskEvalListHead;
static TRSF_DESC *taskEvalListTail;
static TRSF_DESC *freeList;
static perCpuInit = UNDEF;

int transDebug = 0;

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	oldTfxnArgs = 0;
	perCpuInit = UNDEF;
	requestFlagInit (&initDone);
}

static doCpuInit()
{
	/* Called once per program to initialize data structures. */

	int k;

	freeList = trsfPool;
	for (k=0; k<(ARRAY_SIZE(trsfPool)-1); k++)
	 { trsfPool[k].next = &trsfPool[k+1];
	 }
	trsfPool[k].next = NULL;
	taskEvalListHead = NULL;
	taskEvalListTail = NULL;
	perCpuInit = rciCpu;
}

/* dummy functions to identify constant and variable transforms */
constTrsf()
{
}

varbTrsf()
{
}

TRSF *readTrans(t, r)
TRSF *t, *r;
{
	/* Read the value of a transform atomically */

	TRSF_DESC *tr = TRSF_GETDESC(t);

	if (tr->magic != TRSF_MAGIC)
	 { return (NULL);
	 }
	accessMem (tr, t, r, sizeof(TRSF));
	return (t);
}

TRSF *writeTrans(t, r)
TRSF *t, *r;
{
	/* Write the value of a transform atomically */

	TRSF_DESC *tr = TRSF_GETDESC(t);

	if (tr->magic != TRSF_MAGIC)
	 { return (NULL);
	 }
	accessMem (tr, r, t, sizeof(TRSF));
	return (t);
}

TRSF *getTransByName (name)
const char *name;
{
	/* Get transform pointer by name */

	return ((TRSF*)getMemByNameId((void*)name, TRSF_HASH_ID));
}

TRSF *getTransByAddr (t)
TRSF *t;
{
	/* Get transform pointer by address */

	TRSF_DESC *tr = TRSF_GETDESC(t);

	return ((TRSF*)getMemByAddr((void*)tr));
}

char *getTransName (t)
TRSF *t;
{
	/* Get the name of a transform */

	return (getMemName ((TRSF_GETDESC(t))));
}	

transCtrl(td, code)
RCI_DESC *td;
int code;
{
	/* Called at the request of the planning level to change
	   the function binding or argument associated with a 
	   particular transform.
	 */

	char *fxnName = "transCtrl()";

	RCCL_SMEM *smem;
	MOTION_PARAMETER_BLOCK *mpb;
	TRSF_DESC **headp, **tailp, *tr, *parent;

	extern RCCL_GMEM *rcclGmem;

	INIT_CHECK();
	if (perCpuInit != rciCpu)
	 { doCpuInit();
	 }
	if (!globalMemoryCheck(fxnName)) /* validate rcclGmem */
	 { return (-1);
	 }
	if ((smem = (RCCL_SMEM*)getSharedMemory ("RCCL_SMEM")) == NULL)
	 { rciAbort (EInternal, rcclCantGetSmemMsg, fxnName);
	   return (-1);
	 }
	if ((parent = TRSF_GETDESC(getTransByAddr((TRSF*)smem->transPtr))) == NULL)
	 { rciAbort (EInternal, rcclCantGetTrsfMsg, fxnName, smem->transPtr);
	   return (-1);
	 }
	/* We will probably have to work with the list of transforms
	   which are bound to functions. If we are working with a
	   'transMotionEval' type binding, this list is obtained
	   from the associated manipulator. Otherwise, the list is 
	   obtained from the trajectory task (in this case, the variable
	   rcclEvalList). */

	if (code == MANIP_TRANS_PICKUP || code == MANIP_TRANS_DROP)
	 { 
	   if ((mpb = smem->mpb[smem->manipNum]) == NULL)
	    { rciAbort (EInternal, rcclCantGetLmemMsg, fxnName);
	      return (-1);
	    }
	   headp = &mpb->evalListHead;
	   tailp = &mpb->evalListTail;
	 }
	else
	 { headp = &taskEvalListHead;
	   tailp = &taskEvalListTail;
	 }

	/* see if the transform is currently on an eval list ... */
	for (tr = *headp; tr; tr = tr->next)
	 { if (tr->source == smem->transPtr)
	    { break;
	    }
	 }

	if (code == TASK_TRANS_DROP || code == MANIP_TRANS_DROP)
	 { /* Transform should have been found on the list. Remove the
	      binding by clearing the appropriate owner field,
	      unlinking the local copy of the transform from the list, 
	      and placing it back on the free list.
	   */
	   if (!tr)
	    { if (code == TASK_TRANS_DROP)
	       { rciAbort (EInternal, "%s -- null transform\n", fxnName);
		 return (-1);
	       }
	      else
	       { return (1);
	       }
	    }
	   if (code == MANIP_TRANS_DROP)
	    { parent->manipOwner = 0;
	    }
	   else
	    { parent->taskOwner = 0;
	    }
	   UNLINK_HT (*headp, *tailp, tr);
	   if (transDebug)
	    { rciPrintf ("%s unlinking %s\n", code == MANIP_TRANS_DROP ?
			 "manip" : "trans", getMemName(tr->parent));
	    }
	   tr->next = freeList;
	   freeList = tr;
	 }
	else
	 { if (!tr)
	    { if (code == MANIP_TRANS_PICKUP)
	       { POS *p;
		 POS_TERM *t;

		 /* Do a 'transMotionEval()' type binding. If the
		    transform is part of the manipulator's current
		    target position, link it into the evaluation
		    list for 'mnp' immediately.
		  */
		 if ((p = mpb->goalPos) && rcclGmem->taskActive[rciCpu])
		  { if (transDebug)
		     { rciPrintf ("p = 0x%x mpb = 0x%x\n", p, mpb);
		     }
		    for (t=p->t6Term->prev; t!=p->t6Term; t=t->prev)
		     { tr = TRSF_GETDESC(t->trsf);
		       if (tr->fxn == varbTrsf && tr->source == smem->transPtr)
			{ tr->parent = parent;
			  LINK_HT (*headp, *tailp, tr);
	   if (transDebug)
	    { rciPrintf ("manip linking %s\n", getMemName(tr->parent));
	    }
			  readTrans ((TRSF*)parent, (TRSF*)tr);
			  tr->timestamp = 0;
			  break;
			}
		     }
		  }
	       }
	      else
	       { /* Request is to attach the transform to a permanent
		    monitor function. Link it into the list right away.
		  */
		 if ((tr = freeList) == NULL)
		  { rciAbort (EMemory, "%s -- out of links\n", fxnName);
		    return (-1);
		  }
		 else
		  { freeList = tr->next;
		  }
		 tr->source = smem->transPtr;
		 tr->parent = parent;
	   if (transDebug)
	    { rciPrintf ("task linking %s\n", getMemName(tr->parent));
	    }
		 LINK_HT (*headp, *tailp, tr);
		 readTrans ((TRSF*)tr->parent, (TRSF*)tr);
		 tr->timestamp = 0;
	       }
	    }
	   /* set the function/argument field in the parent and make
	      the appropriate marking to indicate the function binding.
	    */
	      
	   parent->fxn = smem->transFxn;
	   parent->arg = smem->transArg;
	   if (code == MANIP_TRANS_PICKUP)
	    { parent->manipOwner = (int)(smem->man);
	      if (transDebug)
	       { rciPrintf ("manip 0x%x picking up\n", smem->man);
	       }
	    }
	   else
	    { parent->taskOwner = TASKNUM(getCurrentTask());
	      if (transDebug)
	       { rciPrintf ("task %d picking up\n", parent->taskOwner);
	       }
	    }
	 }
	return (1);
}

evalTaskTransforms(time)
float time;
{
	/* Called from setpCtrl() to evaluate all transforms which are
	   bound to permanent functions.
	 */

	TRSF_DESC *tr;
	int count = 0;
	TRANS_ARGS targs;		/* for old interface */

	INIT_CHECK();
	if (perCpuInit != rciCpu)
	 { doCpuInit();
	 }
	if (oldTfxnArgs)
	 { targs.scale = 0.0;
	   targs.time = time;
	   targs.manip = NULL;
	 }
	for (tr = taskEvalListTail; tr; tr = tr->prev)
	 { 
	   if (oldTfxnArgs)
	    { targs.arg = tr->parent->arg;
	      (*tr->parent->fxn)(&tr->trsf, &targs);
	    }
	   else
	    { (*tr->parent->fxn)(&tr->trsf, tr->parent->arg, NULL);
	    }
	   writeTrans ((TRSF*)tr->parent, (TRSF*)tr);
	   tr->timestamp = (int)time;
	   count++;
	 }
}

evalManipTransforms(tail, time, scale, mnp)
TRSF_DESC *tail;
float time;
float scale;
MANIP *mnp;
{
	/* Called from setpointControl() to evaluate all transforms which are
	   bound to a per-motion function.
	 */

	TRSF_DESC *tr;
	TRANS_ARGS targs;		/* for old function call interface */

	if (oldTfxnArgs)
	 { targs.scale = scale;
	   targs.time = time;
	   targs.manip = mnp;
	 }
	for (tr = tail; tr; tr = tr->prev)
	 {
	   if (oldTfxnArgs)
	    { targs.arg = tr->parent->arg;
	      (*tr->parent->fxn)(&tr->trsf, &targs);
	    }
	   else
	    { (*tr->parent->fxn)(&tr->trsf, tr->parent->arg, mnp);
	    }
	   writeTrans ((TRSF*)tr->parent, (TRSF*)tr);
	   tr->timestamp = (float)time;
	 }
}

getTransBinding (t, binding)
TRSF *t;
TRANS_BINDING *binding;
{
	/* Find out information about the binding of a particular
	   transform. Return this information in the 'binding'
	   structure.
	 */
	char *fxnName = "getTransBinding()";
	TRSF_DESC *tr = (TRSF_DESC*)t;	/* pointer punning */

	if (tr->magic != TRSF_MAGIC)
	 { errorMsg (EDescriptor, rcclBadTrsfMsg, fxnName, t);
	   RETURN_NORMAL (0);
	 }
	else
	 { if (tr->taskOwner)
	    { binding->code = TRANS_EVAL;
	      binding->mnp = NULL;
	      binding->fxn = tr->fxn;
	      binding->arg = tr->arg;
	    }
	   else if (tr->manipOwner)
	    { binding->code = TRANS_MOTION_EVAL;
	      binding->mnp = (MANIP*)(tr->manipOwner);
	      binding->fxn = tr->fxn;
	      binding->arg = tr->arg;
	    }
	   else if (tr->fxn == constTrsf)
	    { binding->code = TRANS_CONST;
	      binding->mnp = NULL;
	      binding->fxn = NULLF;
	      binding->arg = 0;
	    }
	   else if (tr->fxn == varbTrsf)
	    { binding->code = TRANS_VARB;
	      binding->mnp = NULL;
	      binding->fxn = NULLF;
	      binding->arg = 0;
	    }
	   else
	    { rciAbort (EInternal, rcclBadFxnBindMsg, fxnName, tr->fxn);
	      return (-1);
	    }
	 }
	return (1);
}
