#include <stdio.h>
#include <stdlib.h>

#include <dbDefs.h>
#include <registryFunction.h>
#include <subRecord.h>
#include <aSubRecord.h>
#include <epicsExport.h>
#include <epicsExit.h>
#include <ctype.h>
#include <libgen.h>
#include <unistd.h>
#include <stdint.h>
#include <trbnet.h>
#include <trberror.h>
#include <sys/types.h>

int mySubDebug, trb_inited=0;

int oepVoltage[8][6][4][16];	/* voltage, sector, plane, mbo */

/*static uint16_t tcp_port = 55555;*/
/*static void atexit0(void *arg);*/

static long mytrbInit(aSubRecord *precord)
{
	if(trb_inited == 0) {
 	   if (mySubDebug)
  	      printf("Record %s called mytrbInit(%p)\n",
               precord->name, (void*) precord);
		if (init_ports() == -1) {
			printf("error connecting to trbnet %d\n", trb_errno);
			trb_inited = 0;
		}
		else
			trb_inited=1;
/* install exit handler 
			epicsAtExit(atexit0,0);*/
	}

    return 0;
}
static long oepInit(subRecord *precord)
{
	if(trb_inited == 0) {
 	   if (mySubDebug)
  	      printf("Record %s called oepInit(%p)\n",
               precord->name, (void*) precord);
		if (init_ports() == -1) {
			printf("error connecting to trbnet %d\n", trb_errno);
			trb_inited = 0;
		}
		else
			trb_inited=1;
	}

    return 0;
}

static long mytrbUid(aSubRecord *precord)
{
	uint16_t trb_address;
	int status;
	uint32_t uidBuffer[4];
	if(trb_inited == 0) {
			status = sleep(3);
			if (init_ports() == -1) {
				printf("error connecting to trbnet\n");
				trb_inited = 0;
				precord->val = -1;
				return -1;
			}
			else
				trb_inited=1;
	} 
	trb_address = *(long*) precord->a;
	if (mySubDebug)
		printf("Record %s called mytrbUid(%p) A=0x%X\n",
		       precord->name, (void*) precord, trb_address);

	status = trb_read_uid(trb_address, uidBuffer, 4);
	if (status == -1) {
		printf( "read_uid failed: %d %s\n",trb_errno,
			trb_strerror());
			trb_inited = 0;
	} else {
		printf( "0x%04x  0x%08x  0x%08x  0x%02x\n",
			uidBuffer[3],
			uidBuffer[0], uidBuffer[1], uidBuffer[2]);
		*(uint32_t *)precord->vala=uidBuffer[0];
		*(uint32_t *)precord->valb=uidBuffer[1];
		*(uint32_t *)precord->valc=uidBuffer[2];

	}

	return 0;
}
static long mytrbRead(aSubRecord *precord)
{
	uint16_t trb_address, reg_address;
	int status, count, i;
	uint32_t data[2048], *outpa, *outpb;
	if(trb_inited == 0) {
			status = sleep(3);
			if (init_ports() == -1) {
				printf("error connecting to trbnet\n");
				trb_inited = 0;
				precord->val = -1;
				return -1;
			}
			else
				trb_inited=1;
	} 
	trb_address = *(long*) precord->a;
	reg_address = *(long*) precord->b;
	count = *(long*) precord->c;
	outpa = (uint32_t *)precord->vala;
	outpb = (uint32_t *)precord->valb;
	if (mySubDebug)
		printf("Record %s called mytrbRead(%p) A=0x%X\n",
		       precord->name, (void*) precord, trb_address);

	status = trb_register_read(trb_address, reg_address, data, count*2);
	if (status == -1) {
		printf( "read_register failed:  %d %s\n",trb_errno,
			trb_strerror());
			trb_inited = 0;
		return -1;
	} else {
		if (mySubDebug) printf("trb 0x%04x  reg 0x%04x status %d data %d\n", trb_address, reg_address,
			status,data[0]);
		for (i = 0; i < status; i += 2) {
			if (mySubDebug) printf("0x%04x  0x%08x\n", data[i], data[i + 1]);
			*(outpb++)=data[i]; /* address */
			*(outpa++)=data[i+1]; /* data */
         	}
		if( status <= precord->novb) {
			precord->nevb = status;
		}
		else {
			precord->nevb = precord->novb;
		}
		if( status <= precord->nova) {
			precord->neva = status;
		}
		else {
			precord->neva = precord->nova;
		}
		precord->val = status;
	}

	return 0;
}
static long mytrbReadMem(aSubRecord *precord)
{
	uint16_t trb_address, reg_address;
	int status, count, i, option;
	uint32_t data[5010], *outpa;
	if(trb_inited == 0) {
			status = sleep(3);
			if (init_ports() == -1) {
				printf("error connecting to trbnet\n");
				trb_inited = 0;
				precord->val = -1;
				return -1;
			}
			else
				trb_inited=1;
	} 
	trb_address = *(long*) precord->a;
	reg_address = *(long*) precord->b;
	count = *(long*) precord->c;
	option = *(long*) precord->d;
	outpa = (uint32_t *)precord->vala;
	if (mySubDebug)
		printf("Record %s called mytrbReadMem(%p) A=0x%X\n",
		       precord->name, (void*) precord, trb_address);

	status = trb_register_read_mem(trb_address, reg_address, option, count, data, count*2);
	if (status == -1) {
		printf( "read_register_mem failed: %d %s\n",trb_errno,
			trb_strerror());
			trb_inited = 0;
		return -1;
	} else {
		if (mySubDebug) printf("trb 0x%04x  reg 0x%04x status %d data 0x%08x\n", trb_address, reg_address,
			status,data[0]);
		for (i = 1; i < status; i++) {
			if (mySubDebug) printf("0x%04x  0x%08x\n",reg_address+i-1, data[i]);
			*(outpa++)=data[i]; /* data */
         	}
		if( status <= precord->nova) {
			precord->neva = status;
		}
		else {
			precord->neva = precord->nova;
		}
		precord->val = status;
	}

	return 0;
}
static long oepReadVoltage(subRecord *precord)
{
	uint16_t trb_address, reg_address;
	int status, count, i, iv, is, ip, im;
	uint32_t data[2048];
	/* int oepVoltage[8,6,4,16];	 voltage, sector, plane, mbo */
	if(trb_inited == 0) {
			status = sleep(3);
			if (init_ports() == -1) {
				printf("error connecting to trbnet\n");
				trb_inited = 0;
				precord->val = -1;
				return -1;
			}
			else
				trb_inited=1;
	} 
	trb_address = 0xFFFD;  /* OEP broadcast */
	reg_address = precord->a;
	iv = precord->b; /* voltage number (0-7) */
	count = 384;
	if (mySubDebug)
		printf("Record %s called oepRead(%p) A=0x%X\n",
		       precord->name, (void*) precord, trb_address);
	for (is = 0; is < 6; is++) {
		for (ip = 0; ip < 4; ip++) {
			for (im = 0; im < 16; im++) {
				oepVoltage[iv][is][ip][im] = -1;
			}
		}
	}
	status = trb_register_read(trb_address, reg_address, data, count*2);
	if (status == -1) {
		printf( "read_register failed: %d %s\n",trb_errno,
			trb_strerror());
			trb_inited = 0;
		precord->val = -1;
		return -1;
	} else {
		if (mySubDebug) printf("trb 0x%04x  reg 0x%04x status %d data %d\n", trb_address, reg_address,
			status,data[0]);
		for (i = 0; i < status; i += 2) {
			if (mySubDebug) printf("i: %d  0x%04x  0x%08x\n", i, data[i], data[i + 1]);
			ip = (data[i] & 0x0300) >> 8;
			is = (data[i] & 0x0070) >> 4;
			im = data[i] & 0x000F;
			if (mySubDebug) printf("iv: %d ip: %d is: %d im: %d \n", iv,ip,is,im);
			if(is < 6) oepVoltage[iv][is][ip][im] = data[i + 1];
         	}
		precord->val = status;
	}
	if (mySubDebug) printf("return from oepRead\n");

	return 0;
}
static long oepGetVoltage(subRecord *precord)
{
	int  iv, is, ip, im;
	/* int oepVoltage[8,6,4,16];	 voltage, sector, plane, mbo */
	iv = precord->a; /* voltage number (0-7) */
	is = precord->b; /* sector number (0-5) */
	ip = precord->c; /* plane number (0-3) */
	im = precord->d; /* mbo number (0-15) */
	precord->val = oepVoltage[iv][is][ip][im]/1000.;
	return 0;
}
static long mytrbWrite(aSubRecord *precord)
{
	uint16_t trb_address, reg_address;
	int status;
	uint32_t data;
	if(trb_inited == 0) {
			status = sleep(3);
			if (init_ports() == -1) {
				printf("error connecting to trbnet\n");
				trb_inited = 0;
				precord->val = -1;
				return -1;
			}
			else
				trb_inited=1;
	} 
	trb_address = *(long*) precord->a;
	reg_address = *(long*) precord->b;
	data = *(long*) precord->c;
/*	if (mySubDebug)*/
		printf("Record %s called mytrbWrite(%p) A=0x%X R=0x%4x d=0x%x\n",
		       precord->name, (void*) precord, trb_address,reg_address, data);

	status = trb_register_write(trb_address, reg_address, data);
	if (status == -1) {
		printf( "write_register failed: %d %s\n",trb_errno,
			trb_strerror());
			trb_inited = 0;
		return -1;
	}

	return 0;
}

static long scalerN_init( struct aSubRecord *pgsub )
{
  return( 0 );
}

static long scalerN_proc( struct aSubRecord *pgsub )
{
/* BK this routine performs the scaler summing and profiles for a up to 16 channel array */
	long	*ptr[16], *out[19];
	int	nelm,iscal,jchan,nscal;
	long	totsum,sum;
/*	long	sum_arr[100];
	float	pos_arr[100];*/
	float	pos;
	float *pos_out;

	ptr[0]=  (long *)pgsub->a;	/* input arrays*/
	ptr[1]=  (long *)pgsub->b;
	ptr[2]=  (long *)pgsub->c;
	ptr[3]=  (long *)pgsub->d;
	ptr[4]=  (long *)pgsub->e;
	ptr[5]=  (long *)pgsub->f;
	ptr[6]=  (long *)pgsub->g;
	ptr[7]=  (long *)pgsub->h;
        ptr[8]=  (long *)pgsub->i;
        ptr[9]=  (long *)pgsub->j;
        ptr[10]=  (long *)pgsub->k;
        ptr[11]=  (long *)pgsub->l;
        ptr[12]=  (long *)pgsub->m;
        ptr[13]=  (long *)pgsub->n;
        ptr[14]=  (long *)pgsub->o;
        ptr[15]=  (long *)pgsub->p;
        
	out[0]=  (long *)pgsub->vala;	/* output (0 - 7) A to H are for sums of input arrays*/
	out[1]=  (long *)pgsub->valb;
	out[2]=  (long *)pgsub->valc;
	out[3]=  (long *)pgsub->vald;
	out[4]=  (long *)pgsub->vale;
	out[5]=  (long *)pgsub->valf;
	out[6]=  (long *)pgsub->valg;
	out[7]=  (long *)pgsub->valh;
        out[8]=  (long *)pgsub->vali;
        out[9]=  (long *)pgsub->valj;
        out[10]=  (long *)pgsub->valk;
        out[11]=  (long *)pgsub->vall;
        out[12]=  (long *)pgsub->valm;
        out[13]=  (long *)pgsub->valn;
        out[14]=  (long *)pgsub->valo;
        out[15]=  (long *)pgsub->valp;
	out[16]=  (long *)pgsub->valq;	/* output (16) I are for across sums of input arrays*/
	pos_out=  (float *)pgsub->valr;	/* output (17) J is for weighted mean of inputs, array of same length*/
        out[17]=  (long *)pgsub->vals;	/* output (18) K is profile array, dim max 16*/
        out[18]=  (long *)pgsub->valt;	/* output (19) L is total sum of input arrays*/

	nelm= pgsub->noa;
	nscal= pgsub->nou;
	totsum=0;
	for(iscal=0; iscal<nscal; iscal++)
		{
			sum=0;
			for(jchan=0; jchan < nelm; jchan++)
				{
					sum += *(ptr[iscal]++);
				}
			*out[iscal] = sum;
                        *(out[17]++) = sum;
			totsum += sum;
		}
        *out[18] = totsum;

	ptr[0]=  (long *)pgsub->a;
	ptr[1]=  (long *)pgsub->b;
	ptr[2]=  (long *)pgsub->c;
	ptr[3]=  (long *)pgsub->d;
	ptr[4]=  (long *)pgsub->e;
	ptr[5]=  (long *)pgsub->f;
	ptr[6]=  (long *)pgsub->g;
	ptr[7]=  (long *)pgsub->h;
        ptr[8]=  (long *)pgsub->i;
        ptr[9]=  (long *)pgsub->j;
        ptr[10]=  (long *)pgsub->k;
        ptr[11]=  (long *)pgsub->l;
        ptr[12]=  (long *)pgsub->m;
        ptr[13]=  (long *)pgsub->n;
        ptr[14]=  (long *)pgsub->o;
        ptr[15]=  (long *)pgsub->p;
        
	for(jchan=0; jchan < nelm; jchan++)
		{
			sum = 0;
			pos = 0.;
			for(iscal=0; iscal<nscal; iscal++)
			{
			sum += *(ptr[iscal]);
			pos += iscal * (*(ptr[iscal]++));
			}
			if( sum > 0 )
				pos = pos / sum;
			else
				pos = (nscal-1)/2.;
			*(out[16]++) = sum;
			*pos_out++ = pos;
		}

  return(totsum);
}

static long scalerN_copy( struct aSubRecord *pgsub )
{
/* BK this routine performs the scaler EXTRACTION */
	long	*ptr, *out[19];
	int	nelm,iscal,jchan,nscal;

	ptr =  (long *)pgsub->a;	/* input array*/
	nscal=  pgsub->noa; /* input length */
	jchan=  *(long *)pgsub->c; /*offset */
	nelm=  *(long *)pgsub->d; /*output length */
	if (mySubDebug) {
		printf("Record %s called scalerN_copy(%p) \n",pgsub->name, (void*) pgsub);
		printf(" nscal %d jchan %d nelm %d \n", nscal,jchan,nelm);
	}        
	out[0]=  (long *)pgsub->vala;	/* */
	out[1]=  (long *)pgsub->valb;
	out[2]=  (long *)pgsub->valc;
	out[3]=  (long *)pgsub->vald;
	out[4]=  (long *)pgsub->vale;
	out[5]=  (long *)pgsub->valf;
	out[6]=  (long *)pgsub->valg;
	out[7]=  (long *)pgsub->valh;
        out[8]=  (long *)pgsub->vali;
        out[9]=  (long *)pgsub->valj;
        out[10]=  (long *)pgsub->valk;
        out[11]=  (long *)pgsub->vall;
        out[12]=  (long *)pgsub->valm;
        out[13]=  (long *)pgsub->valn;
        out[14]=  (long *)pgsub->valo;
        out[15]=  (long *)pgsub->valp;
	out[16]=  (long *)pgsub->valq;	
        out[17]=  (long *)pgsub->valr;
        out[18]=  (long *)pgsub->vals;

	for(iscal=0; iscal<nelm; iscal++)
		{
			*out[iscal] = *(ptr + jchan+iscal);
		}
  return(0);

}

static long scaler_rat_init( struct aSubRecord *pgsub )
{
  return( 0 );
}

static long scaler_rat_proc( struct aSubRecord *pgsub )
{
/* BK this routine performs the scaler ratios  */
	long	*ptr[2];
	int	nelm, jchan;
/*	long	sum_arr[100];
	float	pos_arr[100];*/
	float	pos, sum;
	float *pos_out; /* ratio output */

	ptr[0]=  (long *)pgsub->a;	/* input arrays*/
	ptr[1]=  (long *)pgsub->b;  /* divide A by B */
        
	pos_out=  (float *)pgsub->vala;	/* output (1) A is for ratio of inputs, array of same length*/

	nelm= pgsub->noa;
	for(jchan=0; jchan < nelm; jchan++)
		{
			sum = *(ptr[1]++);
			pos = *(ptr[0]++);
			if( sum > 0 )
				pos = pos / sum;
			else
				pos = 0.;
			*pos_out++ = pos;
		}

  return(0);
}
static long ctsRegInit( struct aSubRecord *precord )
{
  return( 0 );
}
static long ctsRegSplit( struct aSubRecord *precord )
{
	uint32_t data, *outpa, *outpb;
	data = *(long*) precord->a;
	outpa = (uint32_t *)precord->vala;
	outpb = (uint32_t *)precord->valb;
	*outpa = 0xffff & data ;
	*outpb = 0xffff & (data >> 16);

  return( 0 );
}
static long ctsRegUnsplit( struct aSubRecord *precord )
{
	uint32_t  ina,inb, *outpa;
	ina  = *(long*) precord->a;
	inb  = *(long*) precord->b;
	outpa = (uint32_t *)precord->vala;
	*outpa = (0xffff & ina) | ((0xffff & inb) <<16) ;

  return( 0 );
}
static long ctsRegSplit4( struct aSubRecord *precord )
{
	uint32_t data, *outpa, *outpb, *outpc, *outpd;
	data = *(long*) precord->a;
	outpa = (uint32_t *)precord->vala;
	outpb = (uint32_t *)precord->valb;
	outpc = (uint32_t *)precord->valc;
	outpd = (uint32_t *)precord->vald;
	*outpa = 0xff & data ;
	*outpb = 0xff & (data >> 8);
	*outpc = 0xff & (data >> 16);
	*outpd = 0xff & (data >> 24);

  return( 0 );
}
static long ctsRegUnsplit4( struct aSubRecord *precord )
{
	uint32_t  ina,inb,inc,ind, *outpa;
	ina  = *(long*) precord->a;
	inb  = *(long*) precord->b;
	inc  = *(long*) precord->c;
	ind  = *(long*) precord->d;
	outpa = (uint32_t *)precord->vala;
	*outpa = (0xff & ina) | ((0xff & inb) <<8)| ((0xff & inc) <<16)| ((0xff & ind) <<24) ;

  return( 0 );
}
static long ctsRegSplit8( struct aSubRecord *precord )
{
	uint32_t data, *outpa, *outpb, *outpc, *outpd;
	uint32_t *outpe, *outpf, *outpg, *outph;
	data = *(long*) precord->a;
	outpa = (uint32_t *)precord->vala;
	outpb = (uint32_t *)precord->valb;
	outpc = (uint32_t *)precord->valc;
	outpd = (uint32_t *)precord->vald;
	outpe = (uint32_t *)precord->vale;
	outpf = (uint32_t *)precord->valf;
	outpg = (uint32_t *)precord->valg;
	outph = (uint32_t *)precord->valh;
	*outpa = 0xf & data ;
	*outpb = 0xf & (data >> 4);
	*outpc = 0xf & (data >> 8);
	*outpd = 0xf & (data >> 12);
	*outpe = 0xf & (data >> 16);
	*outpf = 0xf & (data >> 20);
	*outpg = 0xf & (data >> 24);
	*outph = 0xf & (data >> 28);

  return( 0 );
}
static long ctsRegUnsplit8( struct aSubRecord *precord )
{
	uint32_t  ina,inb,inc,ind,ine,inf,ing,inh, *outpa;
	ina  = *(long*) precord->a;
	inb  = *(long*) precord->b;
	inc  = *(long*) precord->c;
	ind  = *(long*) precord->d;
	ine  = *(long*) precord->e;
	inf  = *(long*) precord->f;
	ing  = *(long*) precord->g;
	inh  = *(long*) precord->h;
	outpa = (uint32_t *)precord->vala;
	*outpa = (0xf & ina) | ((0xf & inb) <<4)| ((0xf & inc) <<8)| ((0xf & ind) <<12) 
		| ((0xf & ine) <<16)| ((0xf & inf) <<20)| ((0xf & ing) <<24)| ((0xf & inh) <<28) ;

  return( 0 );
}
static long WatchInit(subRecord *precord)
{
    return 0;
}
static long WatchReset(subRecord *precord)
{
	printf("Watchdog detected timeout\n");
	kill(getpid(), 9);
    return 0;
}


/* Register these symbols for use by IOC code: */

epicsExportAddress(int, mySubDebug);
epicsRegisterFunction(mytrbInit);
epicsRegisterFunction(mytrbUid);
epicsRegisterFunction(mytrbRead);
epicsRegisterFunction(mytrbReadMem);
epicsRegisterFunction(mytrbWrite);
epicsRegisterFunction(oepInit);
epicsRegisterFunction(oepReadVoltage);
epicsRegisterFunction(oepGetVoltage);
epicsRegisterFunction(scalerN_init);
epicsRegisterFunction(scalerN_proc);
epicsRegisterFunction(scalerN_copy);
epicsRegisterFunction(scaler_rat_init);
epicsRegisterFunction(scaler_rat_proc);
epicsRegisterFunction(ctsRegInit);
epicsRegisterFunction(ctsRegSplit);
epicsRegisterFunction(ctsRegSplit4);
epicsRegisterFunction(ctsRegSplit8);
epicsRegisterFunction(ctsRegUnsplit);
epicsRegisterFunction(ctsRegUnsplit4);
epicsRegisterFunction(ctsRegUnsplit8);
epicsRegisterFunction(WatchInit);
epicsRegisterFunction(WatchReset);
