/*-=-=-=-=-=-=-=-=-=-=-=- 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:	rciCreate
 *
 * DESCRIPTION:	Top level code to implement the rciCreate() function.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1987
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <stdio.h>
#include <signal.h>

#include <ctype.h>

#include "../h/rciTask.h"
#include "../h/robot_jls.h"
#include "../h/errorCodes.h"
#include "../h/robotComfxns.h"
#include "../h/IOdevice.h"
#include "../h/rciOptions.h"

#include "../h/rciConfig.h"
#include "../rci/create.h"
#include "../rci/rciAutoAbort.h"

extern int	rciNumTasks;
extern RCI_DESC rciTaskTable[];

int taskInitDone = 0;
static clockHost = UNDEF;
unsigned long rciControlCpus = 0;

#define TASKNAME(id)	(rciTaskName(&rciTaskTable[id]))

/* ----- Error Messages. Defined externally to save space ----- */

static char *nullMsg		= "%s --\n";
static char *cantOpenCpuMsg	= "%s -- error openning cpu %d\n";
static char *cantLoadCpuMsg	= "%s -- error loading cpu %d\n";
static char *cpusNotAvailMsg	= "%s -- no cpus available (0x%x, 0x%x)\n";
static char *cantLockMemMsg	= "%s -- can't lock down memory\n";
static char *noRCCLEnvMsg	= "%s -- environment variable RCCL not set\n";
static char *nameInUseMsg	= "%s -- name '%s' already in use\n";
static char *cantSetupPtaskMsg	= "%s -- can't set up planning task\n";
static char *cantStartClkMsg	= "%s -- can't start clock\n";
static char *cantExportClkMsg	= "%s -- can't export clock\n";
static char *cantCreateTaskMsg	= "%s -- can't create task on cpu %d\n";
static char *cantImportTaskMsg	= "%s -- can't import task on cpu %d\n";

static doCleanup()
{
	monitorVariableInit();
}

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	extern int rciTaskId;
	extern int rciAbort();

	initDone = 1;
	taskInitDone = 0;
	clockHost = UNDEF;
	rciControlCpus = 0;
	monitorVariableInit();
	levelCheckInit (&rciTaskId, rciAbort);
	requestExitFxn (doCleanup);
	requestFlagInit (&initDone);
}

unsigned long rciAllocCpus (cpuMask)
unsigned long cpuMask;
{
	char *fxnName = "rciAllocCpus()";
	int cpu;
	unsigned long mask;
	unsigned long success = 0;
	int status;

	INIT_CHECK();

	if (!planningLevelCheck (fxnName))
	 { return (-1);
	 }
	cpu = 0;
	for (mask = cpuMask; mask; mask >>= 1)
	 { if (mask & 1)
	    { if (cpu < MAX_CPUS)
	       { if ((status = openCpu (cpu)) < 0)
		  { if (getErrorCode(0) != ENoDev)
		     { errorMsg(getErrorCode(0), cantOpenCpuMsg, fxnName, cpu);
		       RETURN_ERROR (-1);
		     }
		    else
		     { clearErrors();
		     }
		  }
		 if (status == 1)
		  { success |= (1<<cpu);
		  }
	       }
	    }
	   cpu++;
	 }
	return (success);		    
}

RCI_DESC *rciCreate (name, cpuSelect, simflag)
const char *name;
unsigned long cpuSelect;
int simflag;
{
	/* Start of the world for an RCI task. See 'man rciCreate'
	   for a description of the functionality.
	*/

	RCI_DESC *td;
	int n, id;

	int cpu;
	int auxCpus;

	char *fxnName = "rciCreate()";

	INIT_CHECK();

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	 { /* do a floating point operation to enable the floating point unit
	      on systems that care about that sort of thing */
	   double a = 1.0;
	   a = a*a + a;
	 }

	if (getenvRCCL() == NULL)
	 { errorMsg (EEnviron, noRCCLEnvMsg, fxnName);
	   RETURN_ERROR (NULL);
	 }
	if (rciDescriptor (name) != NULL)
 	 { errorMsg (EDuplicate, nameInUseMsg, fxnName, name);
	   RETURN_ERROR (NULL);
	 }

	/* ---- create the planning task if necessary ---- */

	if (rciNumTasks == 0)
	 { if (setupPlanningTask(simflag) < 0)
	    { errorMsg (getErrorCode(0), cantSetupPtaskMsg, fxnName);
	      RETURN_ERROR(NULL);
	    }
	 }
	cpu = UNDEF;

	/* ---- Now pick out a cpu for the task. ---- 
	   if we wish to run the task on an auxiliary cpu, then
	   allocate that cpu, if necessary, and load it with
	   an RCI image
	 */

	if (auxCpus = (cpuSelect & AUXILIARY_MASK))
	 { 
	   unsigned long busy = 0;
	   unsigned long dontExist = 0;
	   int n;

	   n = 0;
	   for ( ; auxCpus; auxCpus >>= 1)
	    { if (auxCpus & 1)
	       { int status;
		 
		 if ((status = openCpu (n)) < 0)
		  { if (getErrorCode(0) == ENoDev)
		     { dontExist |= (1<<n);
		       clearErrors();
		     }
		    else
		     { errorMsg(EBadAccess, cantOpenCpuMsg, fxnName, n);
		       RETURN_ERROR (NULL);
		     }
		  }
		 else if (status == 0)
		  { busy |= (1<<n);
		  }
		 else
		  { if (loadCpu (n) < 0 || setupAuxCpu (n) < 0)
		     { errorMsg (getErrorCode(0), cantLoadCpuMsg, fxnName, n);
		       closeCpu(n);
		       RETURN_ERROR (NULL);
		     }
		    cpu = n;
		    break;
		  }
	       }
	      n++;
	    }
	   if (auxCpus == 0 && !(cpuSelect & ARBITER_CPU))
	    { errorMsg (ENotAvail, cpusNotAvailMsg, fxnName, busy, dontExist);
	      RETURN_ERROR (NULL);
	    }
	 }

	if (cpu == UNDEF)
	 { cpu = ARBITER_CPU;
	   if (!(rciControlCpus & ARBITER_MASK))
	    { /* this is the first arbiter task */

	      if (lockControlMemory() < 0)
	       { errorMsg (getErrorCode(0), cantLockMemMsg, fxnName);
		 RETURN_ERROR (NULL);
	       }
	      if (clockHost == UNDEF)
	       { if (startRciClock (ARBITER_CPU, UNDEF) < 0)
		  { errorMsg (getErrorCode(0), cantStartClkMsg, fxnName);
		    RETURN_ERROR (NULL);
		  }
/*		 printf (">>> starting on arbiter\n");	*/
		 clockHost = ARBITER_CPU;
	       }
	      else if (clockHost != ARBITER_CPU)
	       { if (exportRciClock (clockHost, ARBITER_CPU, 2) < 0)
		  { errorMsg (getErrorCode(0), cantExportClkMsg, fxnName);
		    RETURN_ERROR (NULL);
		  }
/*		 printf (">>> reimporting from %d\n", clockHost);	*/
	       }

	      rciControlCpus |= ARBITER_MASK;
	    }
	 }

	/* create the task */

	id = rciNumTasks;

	if (cpu == ARBITER_CPU)
	 { if (createArbiterTask (id, name) < 0)
	    { errorMsg (getErrorCode(0),
	      		cantCreateTaskMsg, fxnName, ARBITER_CPU);
	      RETURN_ERROR (NULL);
	    }
	 }
	else
	 { if (createAuxiliaryTask (id, name, cpu) < 0)
	    { errorMsg (getErrorCode(0), cantCreateTaskMsg, fxnName, cpu);
	      RETURN_ERROR (NULL);
	    }
	 }

	/* --- now export the task to all other aux cpus --- */

	for (n=0; n<MAX_CPUS; n++)
	 { if (n != ARBITER_CPU && n != cpu && rciControlCpus & (1<<n))
	    { if (importTaskOnAuxiliary (id, n) < 0)
	       { errorMsg (getErrorCode(0), cantImportTaskMsg, fxnName, n);
		 RETURN_ERROR (NULL);
	       }
	    }
	 }

	td = &rciTaskTable[id];

	/* ---- initialize the device to 0 ---- */

	((IO_DEVICE*)td->dev)->defined = NO;

	rciNumTasks++;
	return (td);
}

setupAuxCpu (cpu)
int cpu;
{
	int id;

	if (!(rciControlCpus & (1<<cpu)))
	 { 
	   /* this is a newly allocated CPU */

	   if (setupSharedRegions (cpu) < 0)
	    { return (-1);
	    }
	   for (id=0; id<rciNumTasks; id++)
	    { if (importTaskOnAuxiliary (id, cpu) < 0)
	       { return (-1);
	       }
	    }
	   if (importAllRobots (cpu) < 0)
	    { return (-1);
	    }
	   
	   if (createMsgMemory (cpu) < 0)
	    { return (-1);
	    }
	   if (importMsgMemory (cpu, rciControlCpus | ARBITER_MASK) < 0)
	    { return (-1);
	    }

	   if (clockHost == UNDEF)
	    { if (startRciClock (cpu, UNDEF) < 0)
	       { return (-1);
	       }
/*	      printf (">>> starting on %d\n", cpu);		*/
	      if (exportRciClock (cpu, ARBITER_CPU, 1) < 0)
	       { return (-1);
	       }
	      if (importRciClock (cpu, ARBITER_CPU, NO) < 0)
	       { return (-1);
	       }
	      clockHost = cpu;
	    }
	   else if (clockHost == ARBITER_CPU)
	    { 
/*	      printf (">>> switching to %d from arbiter\n", cpu);	*/
	      if (startRciClock (cpu, ARBITER_CPU) < 0)
	       { return (-1);
	       }
	      if (exportRciClock (cpu, ARBITER_CPU, 2) < 0)
	       { return (-1);
	       }
	      if (importRciClock (cpu, ARBITER_CPU, YES) < 0)
	       { return (-1);
	       }
	      clockHost = cpu;
	    }
	   else
	    { if (exportRciClock (clockHost, cpu, 2) < 0)
	       { return (-1);
	       }
	      if (importRciClock (clockHost, cpu, NO) < 0)
	       { return (-1);
	       }
/*	      printf (">>> importing from %d\n", clockHost);	*/
	    }

	   rciControlCpus |= (1<<cpu);
	 }
	return (1);
}

setupPlanningTask(simflag)
int simflag;
{
#ifdef LINUX_CFG
	extern void onHup();
	extern void onXcpu();
#else
	extern onHup();
	extern onXcpu();
#endif
	mutexInit(ARBITER_CPU);	/* don't wanna forget that! */	      
	atomicOpsInit();	/* don't wanna forget that either ! */
	rciClearAbort();	/* initialize abort stuff */
	signal (SIGHUP, onHup);
	signal (SIGXCPU, onXcpu);
	clearTaskParams();

	if (openCpu (ARBITER_CPU) <= 0)
	 { return (-1);
	 }
	if (setupSharedRegions (ARBITER_CPU) < 0)
	 { return (-1);
	 }
	if (arbMonitorSetup (simflag) < 0)
	 { return (-1);
	 }
	if (createMsgMemory(ARBITER_CPU) < 0)
	 { return (-1);
	 }
	if (createPlanningTask() < 0)
	 { return (-1);
	 }
	putRciName ("planner", 0);
	rciNumTasks++;
	return (1);
}

rciClose()
{
	char *fxnName = "rciClose()";

	INIT_CHECK();

	if (!planningLevelCheck (fxnName))
	 { return (-1);
	 }
	if (rciRelease (~0) < 0)
	 { RETURN_ERROR (-1);
	 }
	clearCpus();
	closeSharedRegions();
	rciControlCpus = 0;
	return (1);
}
