/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
	
	Control level routines for the test program 'rbttest'.

  -------------------------------------------------------------------------*/

#include <sys/time.h>

#include "../h/cdefs.h"	
#include "../h/rciTask.h"
#include "../h/robotControl.h"
#include "../h/rciMonitor.h"
#include "../h/rciMacros.h"

#include "rbttest.h"

ctrlStartup(td)
RCI_DESC *td;
{
	RCB *rcb;
	RCI_RBT *rbt = td->rbts[0];

	if ((rcb = (RCB*)getSharedMemory ("RCB")) == NULL)
	 { rciAbort (0, "startup() -- can't get shared memory RCB\n");
	 }
	td->tcb->sharedMem = (void*)rcb;
	if (rbt)
	 { if ((*rbt->startup)(td, rbt) < 0)
	    { return (-1);
	    }
	   switch (td->tcb->scheduleDisp)
	    { 
	      case ON_TRIGGER:
	       { rciWakeupOn (td, rbt->ready, rbt->readyArg);
		 rciSetTrigger (rbt->wakeup, rbt->wakeupArg);
		 break;
	       }
	      case ON_FUNCTION:
	       { rciWakeupOn (td, rbt->ready, rbt->readyArg);
		 break;
	       }
	      case ON_SIGNAL:
	       { extern int allRobotsReady();
		 rciWakeupOn (td, allRobotsReady, (int)td);
		 if (rcb->cycleCount < rcb->cycleRequest)
		  { simSetTimer (td->tcb->interval, (struct timeval*)0);
		    rcb->cycleCount++;
		  }
		 else
		  { rcb->cycleOff = 1;
		  }
		 break;
	       }
	    }
	 }
	return (1);
}

#define CHECK_ABORT_REQUEST(x) \
	if (rcb->abortRequest == (x)) \
	 { int k; \
	   for (k=0; k<rcb->abortDelay; k++) \
	      ; \
	   rbt->release(td, rbt, RBT_HARDERROR); \
	   rciAbort(0, "requested abort, code %d\n",(x));\
	   return (-1); \
	 }

ctrlFxn(td)
RCI_DESC *td;
{
	RCB *rcb;
	RCI_RBT *rbt = td->rbts[0];
	struct timeval tv0;

	rcb = (RCB*)td->tcb->sharedMem;

	if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { getSystemTime (&tv0);
	 }
	if (rbt)
	 { 
	   CHECK_ABORT_REQUEST(1);
	
	   if (td->tcb->scheduleDisp == ON_CLOCK)
	    { ROBOT_WAKEUP (rbt);
	    }

	   CHECK_ABORT_REQUEST(2);

	   if ((*rbt->input)(td, rbt) < 0)
	    { return (-1);
	    }

	   CHECK_ABORT_REQUEST(3);

	   if ((*rbt->output)(td, rbt) < 0)
	    { return (-1);
	    }

	   CHECK_ABORT_REQUEST(4);

	   if (td->tcb->scheduleDisp == ON_TRIGGER)
	    { rciSetTrigger (rbt->wakeup, (int)rbt->wakeupArg);
	    }
	   else if (td->tcb->scheduleDisp == ON_SIGNAL)
	    { if (rcb->cycleCount < rcb->cycleRequest)
	       { simSetTimer (td->tcb->interval, &tv0);
		 rcb->cycleCount++;
	       }
	      else
	       { rcb->cycleOff = 1;
	       }
	    }

	   CHECK_ABORT_REQUEST(5);

	   if (rcb->abortRequest == 6)
	    { int k;
	      for (k=0; k<rcb->abortDelay; k++)
		 ;
	      rciAbort(0, "requested abort, code %d\n",5);
	    }
	 }
	return (1);
}

ctrlRelease(td, arg, code)
RCI_DESC *td;
int arg, code;
{
	RCB *rcb;
	RCI_RBT *rbt = td->rbts[0];
	int rcode;

	rcb = (RCB*)td->tcb->sharedMem;

	rciSetTrigger (NULLF, 0);
	if (td->tcb->scheduleDisp == ON_SIGNAL)
	 { simSetTimer (0, (struct timeval*)0);
	   rcb->cycleCount = rcb->cycleRequest;
	   rcb->cycleOff = 1;
	 }
	if (code == RELEASE_REQ)
	 { rcode = RBT_NORMAL;
	 }
	else
	 { rcode = RBT_HARDERROR;
	 }
	if (rbt)
	 { if ((*rbt->release)(td, rbt, rcode) < 0)
	    { return (-1);
	    }
	 }
	return (1);
}

allRobotsReady(td)
RCI_DESC *td;
{
	RCI_RBT *rbt;

	if (td->tcb->interruptSet)
	 { int k;
	   td->tcb->interruptSet = 0;
	   for (k=0; k<TASKNUMROBOTS(td); k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if (!(*rbt->ready)(rbt->readyArg))
		  { /* reschedule */
		    simSetTimer (td->tcb->interval/2, (struct timeval *)0);
		    return (0);
		  }
	       }
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }	
}
