/*-=-=-=-=-=-=-=-=-=-=-=- 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 'rbtcom'.

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

#include "../h/rci.h"
#include "rbtcom.h"
#include "../h/timeval.h"

static int argCode;
static CTRL_BLK *ctrlBlk;

#define TRIGGER_SET	1
       
struct timeval lastTrigger;

triggerFxn(td)
RCI_DESC *td;
{
	RCI_RBT *rbt;
	int k;
	int fakeAbort;

	rciSetTrace(0x2);

	fakeAbort = argCode & ABORT_CODE;

	if (fakeAbort == BEFORE_TRIGGER)
	 { rciAbort (fakeAbort, "before triggerFxn\n");
	   goto triggerEnd;
	 }

	for (k=0; k<ctrlBlk->triggerDelay; k++)
	   ;

	lastTrigger = rciWallClock();
	td->tcb->fxnNotice = TRIGGER_SET;
	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { if (ROBOT_WAKEUP(rbt) < 0)
	       { goto triggerEnd;
	       }
	    }
	 }

	if (fakeAbort == AFTER_TRIGGER)
	 { rciAbort (fakeAbort, "after triggerFxn\n");
	   goto triggerEnd;
	 }

triggerEnd:
	rciClearTrace(0x2);
	return;
}

readyFxn(td)
RCI_DESC *td;
{
	RCI_RBT *rbt;
	int robotPresent = 0;
	int k;

	if ((td->tcb->scheduleDisp == ON_TRIGGER && 
	     td->tcb->fxnNotice == TRIGGER_SET) ||
	    (td->tcb->scheduleDisp == ON_FUNCTION))
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	       { robotPresent = 1;
		 if (ROBOT_READY (rbt))
		  { return (1);
		  }
	       }
	    }
	   if (!robotPresent)
	    { return (1);
	    }
	 }
	return (0);
}

do_start(td, arg)
RCI_DESC *td;
int arg;
{
	RCI_RBT *rbt;
	int fakeAbort;
	int k;

	ctrlBlk = (CTRL_BLK*)getMemByAddr((void*)arg);
	argCode = 0;

	fakeAbort = ctrlBlk->abortCode & ABORT_CODE;

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { if ((*rbt->startup)(td, rbt) < 0)
	       {  return (-1);
	       }
	    }
	 }

	if (fakeAbort == AFTER_STARTUP)
	 { rciAbort (fakeAbort, "after startup\n");
	   return(-1);
	 }

	if (td->tcb->scheduleDisp == ON_FUNCTION ||
	    td->tcb->scheduleDisp == ON_TRIGGER)
	 { rciWakeupOn (td, readyFxn, (int)td);
	 }
	if (td->tcb->scheduleDisp == ON_TRIGGER)
	 { rciSetTrigger (triggerFxn, (int)td);
	 }
	return (1);
}


do_run(td, arg)
RCI_DESC *td;
int arg;
{
	RCI_RBT *rbt;
	int k;
	int fakeAbort = ctrlBlk->abortCode & ABORT_CODE;

	rciSetTrace(0x1);

	if (ctrlBlk->delay1)
	 { int k;
	   for (k=0; k<ctrlBlk->delay1; k++)
	      ;
	 }

	if (td->tcb->scheduleDisp == ON_CLOCK)
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if (ROBOT_WAKEUP (rbt) < 0)
		  { return (-1);
		  }
	       }
	    }
	 }

	if (fakeAbort == BEFORE_INPUT && 
	    td->tcb->cycleCount > 10)
	 { rciAbort(fakeAbort, "before input\n");
	   return (-1);
	 }	

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { if ((*rbt->input)(td, rbt) < 0)
	       { return (-1);
	       }
	    }
	 }

	if (fakeAbort == AFTER_INPUT && 
	    td->tcb->cycleCount > 10)
	 { rciAbort (fakeAbort, "after input\n");
	   return (-1);
	 }

	if (ctrlBlk->com)
	 { SET_MAINT_COM (TASKROBOT(td, 0), ctrlBlk->com);
	   ctrlBlk->com = 0;
	 }

	for (k=0; k<MAX_TASKROBOTS; k++)
	 { if (rbt = TASKROBOT(td, k))
	    { 
	      SET_TEST_READ (rbt, 8);
	      PUT_TEST (rbt, 0xaaaa);
	      PUT_TEST (rbt, 0x5555);
	      PUT_TEST (rbt, 0xffff);
	      PUT_TEST (rbt, 0xcccc);

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

	if (fakeAbort == AFTER_OUTPUT &&
	    td->tcb->cycleCount > 10)
	 { int k;
	   for (k=0; k<100; k++)
	      ;
	   rciAbort (fakeAbort, "after output\n");
	   return (-1);
	 }

	argCode = ctrlBlk->abortCode;

	if (ctrlBlk->abortCode == 0x1234 && td->tcb->cycleCount > 100)
	 { *(int*)0 = 5;
	 }

	if (td->tcb->scheduleDisp == ON_TRIGGER)
	 { rciSetTrigger (triggerFxn, (int)td);
	 }

	if (ctrlBlk->delay2)
	 { int k;
	   for (k=0; k<ctrlBlk->delay2; k++)
	      ;
	 }
	if (ctrlBlk->burstDelay && 
	    ((td->tcb->cycleCount % ctrlBlk->burstInterval) == 0))
	 { int k;
	   rciPrintf ("X\n");
	   for (k=0; k<ctrlBlk->burstDelay; k++)
	      ;

	 }
	rciClearTrace(0x1);

	return (1);
}

do_release(td, arg, code)
RCI_DESC *td;
int arg, code;
{
	RCI_RBT *rbt;
	int k;
	int rcode;

	rciSetTrigger (NULLF, 0);

	if (code == RELEASE_REQ)
	 { rcode = RBT_NORMAL;
	 }
	else
	 { rcode = RBT_HARDERROR;
	 }
	if (!(ctrlBlk->abortCode & SILENT_ABORT))
	 { for (k=0; k<MAX_TASKROBOTS; k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if ((*rbt->release)(td, rbt, rcode) < 0)
		  { return (-1);
		  }
	       }
	    }
	 }
	return (1);
}
