/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	rcclOptions
 *
 * DESCRIPTION:	Routines to set and read back RCCL program options.
 *
 *		Also, some obscure system maintenace/tuning routines.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/cdefs.h"
#include "../h/rciOptions.h"
#include "../h/rcclDefs.h"

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

/* If we call these routines BEFORE we have created the global
   memory, we store the options (and parameters) in these statics: */
static unsigned long optionFlags = 0;
/* Tells us which flags have been either set or cleared */
static unsigned long optionFlagsSet = 0;
static float serviceDelay = 0.0;
static int tightControlLoop = NO;
static char *simulatorAddr = NULL;

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	optionFlags = 0;
	optionFlagsSet = 0;
	serviceDelay = 0.0;
	tightControlLoop = NO;
	if (simulatorAddr)
	 { free (simulatorAddr);
	   simulatorAddr = NULL;
	 }
	requestFlagInit (&initDone);
}

extern RCCL_GMEM *rcclGmem;	/* internal global memory blackboard */

extern unsigned long atomicSet();
extern unsigned long atomicClear();
extern unsigned long atomicPut();

rcclInitOptions ()
{
	/* called by the the rcclCreate routine to set up the option
	   flags in global memory */

	INIT_CHECK();

	rcclGmem->optionFlags = optionFlags;
	mutexClear (&rcclGmem->optionsMutex);
}
	
unsigned long rcclPutOptions (mask)
unsigned long mask;
{
	/* RCCL primitive to set all the options. */

	char *fxnName = "rcclPutOptions()";
	unsigned long old;

	INIT_CHECK();

	optionFlagsSet = ~0;
	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { old = optionFlags;
	   optionFlags = mask;
	 }
	else
	 { globalMemoryCheck(fxnName);
	   old = atomicPut (&rcclGmem->optionsMutex, 
			    &rcclGmem->optionFlags, mask); 
	 }
	if (mask & RCCL_ERROR_EXIT)
	 { rciSetOptions (EXIT_ON_ERROR);
	 }
	else
	 { rciClearOptions (EXIT_ON_ERROR);
	 }
	return (old);
}

unsigned long rcclGetOptions (mask)
unsigned long mask;
{
	/* RCCL primitive to get the options selected by 'mask'. */

	char *fxnName = "rcclGetOptions()";
	unsigned long *op;

	INIT_CHECK();

	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { op = &optionFlags;
	 }
	else
	 { globalMemoryCheck(fxnName);
	   op = &rcclGmem->optionFlags;
	 }
	return (*op & mask);
}

unsigned long rcclSetOptions (mask)
unsigned long mask;
{
	/* RCCL primitive to set the options specified by 'mask'. */

	char *fxnName = "rcclSetOptions()";
	unsigned long old;

	INIT_CHECK();

	optionFlagsSet = mask;
	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { old = optionFlags;
	   optionFlags |= mask;
	 }
	else
	 { globalMemoryCheck(fxnName);
	   old = atomicSet (&rcclGmem->optionsMutex, 
			    &rcclGmem->optionFlags, mask); 
	 }
	if (mask & RCCL_ERROR_EXIT)
	 { rciSetOptions (EXIT_ON_ERROR);
	 }
	return (old & mask);
}

unsigned long rcclClearOptions (mask)
unsigned long mask;
{
	/* RCCL primitive to clear the options specified by 'mask'. */

	char *fxnName = "rcclClearOptions()";
	unsigned long old;
	unsigned long *op;

	INIT_CHECK();

	optionFlagsSet = mask;
	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { old = optionFlags;
	   optionFlags &= ~mask;
	 }
	else
	 { globalMemoryCheck(fxnName);
	   old = atomicClear (&rcclGmem->optionsMutex, 
			      &rcclGmem->optionFlags, mask); 
	 }
	if (mask & RCCL_ERROR_EXIT)
	 { rciClearOptions (EXIT_ON_ERROR);
	 }
	return (old & mask);
}

unsigned rcclGetOptionsSet()
{
	/* used internally by the system to see which options flags
	   have been either set OR cleared. WARNING -- can only be
	   called from arbiter CPU. */

	INIT_CHECK();

	return (optionFlagsSet);
}

rcclSetMotionDelay (time)
float time;
{
	/* Sets an instrinsic delay time for the trajectory generator
	   to service a motion request.

	   This is basically a hack system maintenance routine. 

	   We offer the feature because a small delay may sometimes 
	   be desirable, if the planning level is slow and we want
	   to ensure that consecutive motion requests will be handled
	   consecutively by the trajectory generator, without an
	   IDLE state cropping up in between.

	   This routine probably won't be very useful, but you never know.
	 */

	char *fxnName = "rcclSetMotionDelay()";

	INIT_CHECK();

	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { serviceDelay = time;
	 }
	else
	 { globalMemoryCheck(fxnName);
	   rcclGmem->serviceDelay = time;
	 }
}

float rcclGetMotionDelay()
{
	/* Get the instrinsic delay time for the trajectory generator
	   to service a motion request. */

	char *fxnName = "rcclGetMotionDelay()";

	INIT_CHECK();

	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { return (serviceDelay);
	 }
	else
	 { globalMemoryCheck(fxnName);
	   return (rcclGmem->serviceDelay);
	 }
}

rcclTightControlLoop (set)
int set;
{
	/* The trajectory generator normally sends commands to the
	   robot the cycle FOLLOWING the one in which they are
	   computed. This allows the computation of commands 
	   and the servicing of them by the robot controller to
	   overlap. However, it creates delays that may not be
	   good when we are doing feedback control. This routine
	   allows to make the trajectory generator output its
	   commands during the SAME cycle they are computed.
	 */

	char *fxnName = "rcclTightControlLoop()";
	int old;

	INIT_CHECK();

	if (INSIDE_PLANNING_TASK() && rcclGmem == NULL)
	 { old = tightControlLoop;
	   if (set > 0)
	    { tightControlLoop = set;
	    }
	 }
	else
	 { globalMemoryCheck(fxnName);
	   old = rcclGmem->tightControlLoop;
	   if (set > 0)
	    { rcclGmem->tightControlLoop = set;
	    }
	 }
	return (old);
}

rcclSetSimulator(name)
const char *name;
{
	char *fxnName = "rcclGetSimulator()";

	/* Called to set the name of the simulator program to be used
	   be an RCCL program. Must be called before the first call to RCCL
	   create. WARNING -- this code, and the code for rcclGetSimulator(),
	   will break if used on a CPU other than the arbiter, since
	   "simulatorAddr" is not copied to any other CPUs. */
	
	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (simulatorAddr)
	 { free(simulatorAddr);
	 }
	if (name)
	 { simulatorAddr = (char*)strcpy((char*)malloc(strlen(name)+1),name);
	   rcclSetOptions (RCCL_SIMULATE);
	 }
	else
	 { rcclClearOptions (RCCL_SIMULATE);
	 }
	return (1);
}

char *rcclGetSimulator()
{
	char *fxnName = "rcclGetSimulator()";
	INIT_CHECK();

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

rcclAutoAbortCheck()
{
	/* Called to implement the RCCL auto abort feature -- if
	   the RCCL_ERROR_EXIT option has been selected, then 
	   either print errors and exit (planning task) or call
	   rciAbort() (control task).
	 */
	
	char *fxnName = "rcclAutoAbortCheck()";

	INIT_CHECK();

	if (INSIDE_PLANNING_TASK())
	 { if ( (rcclGmem == NULL && (optionFlags & RCCL_ERROR_EXIT)) ||
		(rcclGmem != NULL && (rcclGmem->optionFlags&RCCL_ERROR_EXIT)))
	    { printErrors();
	      exit(-1);
	    }
	 }
	else
	 { globalMemoryCheck(fxnName);
	   if (rcclGmem->optionFlags & RCCL_ERROR_EXIT)
	    { rciAbortErrors();
	    }
	 }
}
