#include "../h/cdefs.h"
#include "robotsim.h"

static numItems ();
static enqueue ();
static dequeue ();

initGenericIO (r)
RobotDesc *r;
{
	/* initialize all the generic IO devices */

	SLU *sbuf;
	int k;

	r->sio.numSlu = 2;
	sbuf = (SLU*)calloc(r->sio.numSlu,sizeof(SLU));
	r->sio.sluBuf = sbuf;
	for (k=0; k<r->sio.numSlu; k++, sbuf++)
	 { sbuf->r.buf = (char*)malloc(SERIAL_BUF_SIZE);
	   sbuf->r.size = SERIAL_BUF_SIZE;
	   sbuf->w.buf = (char*)malloc(SERIAL_BUF_SIZE);
	   sbuf->w.size = SERIAL_BUF_SIZE;
	 }

	r->sio.numADC = 0;
	r->sio.numPioOut = 2;
	r->sio.pioOutBuf = (unsigned short*)calloc(r->sio.numPioOut, 
						   sizeof(short));
	r->sio.numPioIn = 2;
	r->sio.pioInBuf = (unsigned short*)calloc(r->sio.numPioIn, 
						  sizeof(short));
	r->sio.numDAC = 2;
	r->sio.DACBuf = (int*)calloc(r->sio.numDAC,sizeof(int));
	r->sio.numADC = 2;
	r->sio.ADCBuf = (int*)calloc(r->sio.numADC,sizeof(int));
}

/* Queue support routines, for serial line units. 
   We add at the tail and remove at the head */

static numItems (q)
QUEUE *q;
{
	if (q->tail >= q->head)
	 { return (q->tail - q->head);
	 }
	else
	 { return (q->size - (q->head - q->tail));
	 }
}

#define QUEUE_FULL(q)	(numItems(q) == ((q)->size-1))
#define QUEUE_EMPTY(q)	((q)->head == (q)->tail)
#define QUEUE_SPACE(q)	((q)->size - numItems(q) - 1)

static enqueue (q, buf, size)
QUEUE *q;
char *buf;
int size;
{
	/* add items to a queue, truncating if necessary */

	int count, k, i;

	count = MIN(QUEUE_SPACE(q),size);
	for (k=0, i=q->tail; k<count; k++, i=((i+1)%q->size) )
	 { q->buf[i] = buf[k];
	 }
	q->tail = i;
	return (count);
}

static dequeue (q, buf, size)
QUEUE *q;
char *buf;
int size;
{
	/* remove items from a queue, truncating if necessary */

	int count, k, i;

	count = MIN(numItems(q),size);
	for (k=0, i=q->head; k<count; k++, i=((i+1)%q->size) )
	 { buf[k] = q->buf[i];
	 }
	q->head = i;
	return (count);
}

getSlu (r, chan, buf, size)
RobotDesc *r;
int chan;
char *buf;
int size;
{
	/* read back bytes from serial line unit 'chan' */

	SLU *slu = &r->sio.sluBuf[chan];

	return (dequeue (&slu->r, buf, size));
}

putSlu (r, chan, buf, size)
RobotDesc *r;
int chan;
char *buf;
int size;
{
	/* write bytes to serial line unit 'chan' */

	SLU *slu = &r->sio.sluBuf[chan];

	if (slu->active)
	 { return (enqueue (&slu->w, buf, size));
	 }
	else
	 { return (0);
	 }
}

sluStart (r, chan)
RobotDesc *r;
int chan;
{
	SLU *slu = &r->sio.sluBuf[chan];

       	slu->r.head = slu->r.tail = 0;
	slu->w.head = slu->w.tail = 0;
	slu->active = 1;
}

sluStop (r,chan)
RobotDesc *r;
int chan;
{
	r->sio.sluBuf[chan].active = 0;
}

unsigned short getPioIn (r, chan)
RobotDesc *r;
int chan;
{
	return (r->sio.pioInBuf[chan]);
}

putPio (r, chan, value)
RobotDesc *r;
int chan;
unsigned short value;
{
	r->sio.pioOutBuf[chan] = value;
}

unsigned short getPioOut (r, chan)
RobotDesc *r;
int chan;
{
	return (r->sio.pioOutBuf[chan]);
}

putDAC (r, chan, value)
RobotDesc *r;
int chan;
short value;
{
	if (0 <= chan && chan < r->sio.numDAC)
	 { r->sio.DACBuf[chan] = value;
	 }
}

short getADC (r, chan)
RobotDesc *r;
int chan;
{
	if (0 <= chan && chan < r->sio.numADC)
	 { return ((short)(r->sio.ADCBuf[chan]));
	 }
}

/*---------------------------------------------------------------------------
 * SIMULATOR support routines: used by the simulator emulation code
 *--------------------------------------------------------------------------*/

loadSlu (r, chan, buf, size)
RobotDesc *r;
int chan;
char *buf;
int size;
{
	/* SIMULATOR side:
	   load data into the read buffer of serial line unit 'chan' */

	SLU *slu = &r->sio.sluBuf[chan];

	if (slu->active)
	 { return (enqueue (&slu->r, buf, size));
	 }
	else
	 { return (0);
	 }
}

emptySlu (r, chan, buf, size)
RobotDesc *r;
int chan;
char *buf;
int size;
{
	/* SIMULATOR side:
	   empty data from the write buffer of serial line unit 'chan' */

	SLU *slu = &r->sio.sluBuf[chan];

	if (slu->active)
	 { return (dequeue (&slu->w, buf, size));
	 }
	else
	 { return (0);
	 }
}

sluReadSpace (r, chan)
RobotDesc *r;
int chan;
{
	/* SIMULATOR side:
	   return the amount of space in the read queue */

	QUEUE *q = &r->sio.sluBuf[chan].r;

	return (QUEUE_SPACE(q));
}

sluWriteItems (r, chan)
RobotDesc *r;
int chan;
{
	/* SIMULATOR side:
	   return the number of items in the write queue */

	QUEUE *q = &r->sio.sluBuf[chan].w;

	return (numItems(q));
}
