/*
 gcc -Wall -o srureadout-busy srureadout-busy.c

srureadout --rate <Hz> --datafile <filename to write data> --events <triggers to take> --sruif <network interface> --ipset
Options:
--datafile,-d <output file name> default none
--events,-e <events to take> 0=infite which is default
--fec,-f <front end card/DTC link> can be given many times (no default)
--help HELP
--ipset,-i write local ip to SRU register (default off)
--sruif,-n <network interface> default = eth0
--rate,-r <trigger rate in Hz, 0 = fast as possible> default=10
--softtrigger,-s use software trigger (default hardware)

example 100 events with software trigger, using standard eth0 interface as fast as possible, logging data.
./srureadout --rate 0 --data somefile.dat --event 100 --fec 0

NOTE:
SRU must be initialized before use of srureadout, e.g. with srupup.
SRU ipnumbers are fixed in code to 192.168.4.4 and 192.168.4.5.

 */
#include <stdlib.h>
#include <getopt.h>
#include <signal.h>
#include <sys/signalfd.h>
#include <stdio.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <errno.h>
#include <poll.h>
#include <ifaddrs.h>
#include <netdb.h>


#define SRU_WAIT_WRITE 300
#define SRU_LOCAL_IP "192.168.4.11"
#define SRU_CONTROL_IP "192.168.4.4"
#define SRU_READOUT_IP "192.168.4.5"
#define NODESEL_SRU  0x00100000
#define RD_COMMAND   0x80000000 // Read command
#define SRU_CONTROL_PORT 4097
//#define SRU_LISTEN_PORT 4101 // for standalone programs
#define SRU_LISTEN_PORT 4098 // for readout programs
#define SRU_READOUT_PORT 6006


#define SRU_REG_DTCRDOMASK1    0x23 // Readout mask of DTC   0-19 links
#define SRU_REG_DTCRDOMASK2    0x24 // Readout mask of DTC  20-39 links
#define SRU_REG_STRIG_CMD 0x42 // Send one software trigger
#define SRU_REG_TRIG_CNT_CLR   0x43 // All trigger counters clear
#define SRU_REG_READOUT_IP 0x08 // IP address of local machine
#define SRU_REG_ALLOWED_EVENTS 0x46 // Counts down each trigger. When 0, masks further triggers.

//#define SRU_IF "wlp5s0"
#define SRU_IF "eth0"

typedef struct {
  char controlip[64];
  int controlport;
  struct sockaddr_in si_sru;
  int readoutport;
  char interface[64];
  char readoutip[64];
  unsigned int dtcrdomask1;
  unsigned int dtcrdomask2;
} SRUSERVER;

typedef struct {
  struct sockaddr_in si_controlme;
  int controlsocket; // socket for control
  int readoutsocket; // socket for readout
  int listenport; // port for listen on control
  char localip[64];
  char interface[64]; // interface - to be used in future
  char broadcastip[64];
  int nbevents;
  SRUSERVER sruserver;
} SRU;
SRU srus;

FILE *fout;
char logfile[128];
int sru_decode_data = 0;
int signal_stop = 0;
int software_trigger = 0;
int rate = 10;
int set_sruif = 0;
int set_sruip = 0;
char ip_dest[20]; // IP address of destination
char sruif[64];

int busyon = 0;

char datafile[256];
int datalogging = 0;
FILE *dout;

// Data buffer
#define EVTHEADER_LENGTH 3 // total length + event id + SRU ip address
#define BUFLEN 5000000
unsigned char evtbuffer[BUFLEN+EVTHEADER_LENGTH+1];
int nbevents = 0;
// buffer for polling networkdata
struct pollfd runconnect[2];
sigset_t sigset;
int signal_fd;
struct signalfd_siginfo siginfo;

int decodeParameters(int argc, char **argv);
int sruWriteRegister(int handle,SRUSERVER *srv,int reg, unsigned int value);
int scSruConnect(SRU *srus);
int dataSruConnect(SRU *srus);
int dataSruReceiver(SRU *srus);
int findip(char *interface, char *ipaddress, char *bcaddress);
int sruLoadReadoutList(int handle,SRUSERVER *srv);
int sruReadRegister(int handle,SRUSERVER *srv,int reg, unsigned int *value);


int decodeParameters(int argc, char **argv)
{
  char *short_options = "bd:e:f:hin:r:s:";
  int option_index = 0;
  int fecnb;
  int c;
  const struct option long_options[]=
    {
      {"busyon",0,0,'b'},
      {"datafile",1,0,'d'},
      {"events",1,0,'e'},
      {"fec",1,0,'f'},
      {"help",0,0,'h'},
      {"ipset",0,0,'i'},
      {"sruif",1,0,'n'},
      {"rate",1,0,'r'},
      {"softtrigger",0,0,'s'},
      {0,0,0,0},
    };
  strcpy(sruif,SRU_IF);

  strcpy(ip_dest,SRU_CONTROL_IP);
  srus.sruserver.dtcrdomask1 = 0;
  srus.sruserver.dtcrdomask2 = 0;
  nbevents = 0;
  set_sruif = 0;
  if (argc > 1)
    {
      while ((c = getopt_long(argc, argv, short_options,
                              long_options, &option_index)) != EOF)
        {
          switch (c)
            {
	    case 'b': busyon = 1; break;
	    case 'd': strncpy(datafile,optarg,sizeof(datafile));
	      datalogging = 1;
	      break;
	    case 'e': sscanf(optarg,"%d",&nbevents); break;
	    case 'f':
	      sscanf(optarg,"%d",&fecnb);
	      if ((fecnb >=0 ) && (fecnb < 20)) {
		srus.sruserver.dtcrdomask1 |= (1 << fecnb);
	      }
	      else if ((fecnb >=20 ) && (fecnb < 40)) {
		fecnb -= 20;
		srus.sruserver.dtcrdomask2 |= (1 << fecnb);
	      }
	      else {
		printf("%s: fec number must be between 0 and 39, is %d\n",__FUNCTION__,fecnb);
		return 2;
	      }
	      break;
	    case 'h':
	      puts("Options:");
	      puts("--datafile,-d <output file name> default none");
	      puts("--events,-e <events to take> 0=infite which is default");
	      puts("--fec,-f <front end card/DTC link> can be given many times (no fdefault)");
	      puts("--help HELP");
	      puts("--ipset,-i write local ip to SRU register (default off)");
	      printf("--sruif,-n <network interface> default = %s\n",sruif);
	      printf("--rate,-r <trigger rate in Hz, 0 = fast as possible> default=%d\n",rate);
	      puts("--softtrigger,-s use software trigger (default hardware)");
	      return 1;
	      break;
	    case 'i': set_sruip = 1; break;
	    case 'n':
	      strncpy(sruif,optarg,sizeof(sruif));
	      set_sruif = 1;
	      break;
	    case 'r': sscanf(optarg,"%d",&rate); break;
	    case 's': software_trigger = 1; break;
            default:
              printf("%s: Unknown option %s\n",__FUNCTION__,argv[optind-1]);
              return 1;
            }
        }
    }
  if ((srus.sruserver.dtcrdomask1 == 0) && (srus.sruserver.dtcrdomask2 == 0)) {
    printf("%s: No fec between 0 and 39, is given\n",__FUNCTION__);
    return 3;
  }
  if (rate < 0) {
    printf("%s: rate must be >= 0, is given: %d\n",__FUNCTION__,rate);
    return 3;
  }
  return 0;
}

/* Network interface */

int findip(char *interface, char *ipaddress, char *bcaddress)
{
    struct ifaddrs *ifaddr, *ifa;
    int s;
    int iffound = 0;

    if (getifaddrs(&ifaddr) == -1) {
        return errno;
    }


    for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) {
        if (ifa->ifa_addr == NULL)  continue;  

        if((strcmp(ifa->ifa_name,interface)==0)&&(ifa->ifa_addr->sa_family==AF_INET)) {
          s=getnameinfo(ifa->ifa_addr,sizeof(struct sockaddr_in),ipaddress, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
          if (s != 0) {
            return errno;
          }
          s=getnameinfo(ifa->ifa_broadaddr,sizeof(struct sockaddr_in),bcaddress, NI_MAXHOST, NULL, 0, NI_NUMERICHOST);
          if (s != 0) {
            return errno;
          }
          iffound++;
        }
    }
    freeifaddrs(ifaddr);
    if (iffound != 1) return -(1+iffound);
    return 0;
}


/* SRU part */

int sruReadRegister(int handle,SRUSERVER *srv,int reg, unsigned int *value)
{
  unsigned int buf[20];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  struct sockaddr_in si_from;
  socklen_t si_fromlen=sizeof(si_from);
  int mynode,myreg;
  buf[0] = htonl(NODESEL_SRU);
  buf[1] = 0;
  buf[2] = htonl(reg | 0x80000000);
  buf[3] = 0;
  s_bytes = 16;
  if ( (s_sent = sendto(handle, buf, 16, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  if ((s_bytes = recvfrom(handle, buf,sizeof(buf), 0, (struct sockaddr *) &si_from, &si_fromlen))==-1) {
    return -3;
  }
  if (s_bytes != 12) {
    return -4;
  }

  if (si_from.sin_addr.s_addr == srv->si_sru.sin_addr.s_addr) {
    printf("%s: Received packet length %d from %s: %d\n",__FUNCTION__,s_bytes, 
             inet_ntoa(si_from.sin_addr), ntohs(si_from.sin_port));
  }
  else {
    printf("%s: Received illegal packet length %d from %s: %d\n",__FUNCTION__,s_bytes, 
           inet_ntoa(si_from.sin_addr), ntohs(si_from.sin_port));
    return -5;
  }
  
  mynode = ntohl(buf[0]);
  if (mynode != 41) {
    printf("%s: Received not expected node %08X expected %08X\n",
           __FUNCTION__,mynode,41);
    return -6;
  }

  myreg = ntohl(buf[1]);

  if (myreg != (reg | 0x80000000)) {
    printf("%s: Received not expected register %08X expected %08X\n",
           __FUNCTION__,myreg,reg | 0x80000000);
    return -7;
  }

  *value = ntohl(buf[2]);
  printf("%s: %08X %08X %08X\n",__FUNCTION__,mynode,myreg,*value);
  return 0;
}



int sruWriteRegister(int handle,SRUSERVER *srv,int reg, unsigned int value)
{
  unsigned int buf[4];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  buf[0] = htonl(NODESEL_SRU);
  buf[1] = 0;
  buf[2] = htonl(reg);
  buf[3] = htonl(value);
  s_bytes = 16;
  if ( (s_sent = sendto(handle, buf, 16, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  usleep(SRU_WAIT_WRITE);
  return 0;
}

/* load which mcm to enable from the RU
*/
int sruLoadReadoutList(int handle,SRUSERVER *srv)
{
  unsigned int dtcrdomask1r;
  unsigned int dtcrdomask2r;
  FILE *fout;
  int err;

  if ((fout = fopen("/tmp/srureadout.sru","w")) == NULL) return -1;
  fprintf(fout,"%s\n",srv->controlip); 
  fprintf(fout,"%08X\n%08X\n",NODESEL_SRU,0);
  fprintf(fout,"%08X\n%08X\n",SRU_REG_DTCRDOMASK1,srv->dtcrdomask1);
  fprintf(fout,"%08X\n%08X\n",SRU_REG_DTCRDOMASK2, srv->dtcrdomask2);
  fprintf(fout,"%08X\n%08X\n",RD_COMMAND | SRU_REG_DTCRDOMASK1,0);
  fprintf(fout,"%08X\n%08X\n",RD_COMMAND | SRU_REG_DTCRDOMASK2,0);
  fclose(fout);
  err = sruWriteRegister(handle,srv,SRU_REG_DTCRDOMASK1,srv->dtcrdomask1);
  if (err != 0) return err;
  err = sruWriteRegister(handle,srv,SRU_REG_DTCRDOMASK2,srv->dtcrdomask2);
  if (err != 0) return err;
  err = sruReadRegister(handle,srv,SRU_REG_DTCRDOMASK1,&dtcrdomask1r);
  if (err != 0) return err;
  err = sruReadRegister(handle,srv,SRU_REG_DTCRDOMASK2,&dtcrdomask2r);
  if (err != 0) return err;
  if (srv->dtcrdomask1 != dtcrdomask1r) return dtcrdomask1r | 0x10000;
  if (srv->dtcrdomask2 != dtcrdomask2r) return dtcrdomask2r | 0x20000;
  return 0;
}


/*

int scSruConnect(SRU *srus)
{
  int s;
  struct timeval timeout;
  timeout.tv_sec = 0;
  timeout.tv_usec = 100000;

  if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
    printf("%s: socket error %s\n",__FUNCTION__,strerror(errno));
    return -1;
  }
  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO,&timeout,sizeof(timeout));
  memset((char *) &srus->si_controlme, 0, sizeof(struct sockaddr));
  srus->si_controlme.sin_family = AF_INET;
  srus->si_controlme.sin_port = htons(srus->listenport);
  if (inet_aton(srus->localip, &srus->si_controlme.sin_addr)==0) {
    printf("%s: inet_aton error %s\n",__FUNCTION__,strerror(errno));	
    close(s);
    return -2;
  }
  if (bind(s, (struct sockaddr *)&srus->si_controlme, sizeof(struct sockaddr)) != 0) {
    printf("%s: %s port %d bind error %s\n",__FUNCTION__,srus->localip,srus->listenport,strerror(errno));	
    close(s);
    return -3;
  }
  srus->controlsocket = s;

  memset((char *) &srus->sruserver.si_sru, 0, sizeof(struct sockaddr_in));
  srus->sruserver.si_sru.sin_family = AF_INET;
  srus->sruserver.si_sru.sin_port = htons(srus->sruserver.controlport);
  if (inet_aton(srus->sruserver.controlip, &srus->sruserver.si_sru.sin_addr)==0) {
    printf("%s: sru %s port %d inet_aton error %s\n",__FUNCTION__,srus->sruserver.controlip,
	   srus->sruserver.controlport,strerror(errno));	
    close(s);
    srus->controlsocket = 0;
    return -4;
  }
  return 0;
}

*/
int scSruConnect(SRU *srus)
{
  int s;
  struct timeval timeout;
  timeout.tv_sec = 0;
  timeout.tv_usec = 100000;

  if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
    printf("%s: socket error %s\n",__FUNCTION__,strerror(errno));
    return -1;
  }
  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO,&timeout,sizeof(timeout));
  memset((char *) &srus->si_controlme, 0, sizeof(struct sockaddr));
  srus->si_controlme.sin_family = AF_INET;
  srus->si_controlme.sin_port = htons(srus->listenport);
  //  if (inet_aton(srus->sruserver[0].controlip, &srus->si_controlme.sin_addr)==0) {
  if (inet_aton(srus->localip, &srus->si_controlme.sin_addr)==0) {
    printf("%s: %s %d inet_aton error %s\n",__FUNCTION__,srus->sruserver.controlip,srus->listenport,strerror(errno));    
    close(s);
    return -2;
  }
  if (bind(s, (struct sockaddr *)&srus->si_controlme, sizeof(struct sockaddr)) != 0) {
    printf("%s: %s %d bind error %s\n",__FUNCTION__,srus->sruserver.controlip,srus->listenport,strerror(errno)); 
    close(s);
    return -3;
  }
  srus->controlsocket = s;

  memset((char *) &srus->sruserver.si_sru, 0, sizeof(struct sockaddr_in));
  srus->sruserver.si_sru.sin_family = AF_INET;
  srus->sruserver.si_sru.sin_port = htons(srus->sruserver.controlport);
  if (inet_aton(srus->sruserver.controlip, &srus->sruserver.si_sru.sin_addr)==0) {
    printf("%s: sru inet_aton error %s\n",__FUNCTION__,strerror(errno));        
    close(s);
    srus->controlsocket = 0;
    return -4;
  }
  return 0;
}

int dataSruConnect(SRU *srus)
{
  char msgbuf[512];
  int ro_sockfd;
  struct sockaddr_in d_sru;

  printf("%s: %s port %d\n",__FUNCTION__,srus->localip,srus->sruserver.readoutport);

  
  if ((ro_sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
    sprintf(msgbuf, "%s: readout socket error %s\n",__FUNCTION__,strerror(errno));
    printf("%s",msgbuf);
    return -1;
  }
  memset((char *) &d_sru, 0,sizeof(d_sru));
  d_sru.sin_family = AF_INET;
  d_sru.sin_port = htons(srus->sruserver.readoutport);
  //d_sru.sin_addr.s_addr = htonl(INADDR_ANY);
  //d_sru.sin_addr.s_addr = inet_addr(srus->localip);
  //   if (inet_aton(SRU_LOCAL_IP, &d_sru.sin_addr)==0) {
  //   if (inet_aton("192.168.0.14", &d_sru.sin_addr)==0) {
  
  if (inet_aton(srus->localip, &d_sru.sin_addr)==0) {
    sprintf(msgbuf, "%s: %s port %d readout inet_aton error %s\n",
	    __FUNCTION__,srus->localip,srus->sruserver.readoutport,strerror(errno));   
    printf("%s",msgbuf);
    close(ro_sockfd);
    return -2;
  }
  
  if (bind(ro_sockfd, (struct sockaddr *)&d_sru,sizeof(struct sockaddr)) ==-1) {
    sprintf(msgbuf, "%s: %s port %d readout socket bind error %s\n",
	    __FUNCTION__,srus->localip,srus->sruserver.readoutport,strerror(errno)); 
    printf("%s",msgbuf);
    close(ro_sockfd);
    return -3;
  }

  srus->readoutsocket = ro_sockfd;
  printf("%s: %s port %d\n",__FUNCTION__,srus->localip,srus->sruserver.readoutport);
   return 0;
  
}

int dataSruReceiver(SRU *srus)
{
  char msgbuf[512];
  int istat;
  int timeout;
  int delay;
  int evtreceived = 0;
  int newevent;
  int evtpackets;
  struct sockaddr_in s_sru;
  socklen_t s_length=sizeof(s_sru);

  //  socklen_t s_length;
  int r_length;
  int data_length;
  int total_length;
  unsigned int *evtheader;
  unsigned int *data_buffer;
  int v4,v3,v2,v1;
  unsigned int ipaddress;

  runconnect[0].fd = srus->readoutsocket;
  runconnect[0].events = POLLIN;


  if (rate > 0)
    delay = 1000000/rate; // time between triggers in microseconds
  else
    delay = 1;
  if (busyon == 0) {
      istat = sruWriteRegister(srus->controlsocket,&srus->sruserver,SRU_REG_ALLOWED_EVENTS,0xFFFFFFFF);    
  }
  while ((srus->nbevents == 0) || (evtreceived < srus->nbevents)) {
    if (busyon != 0) {
    /* 20251007 JUM
      ALLOWED_EVENTS (register 0x46):
      Counts down each trigger. When 0, masks further triggers.
      Use to stall triggers from the SW side. Write 0x01h to allow a single trigger.
      Enable this function by writing a '1' in SRU_CFG[6]: done in sru configfile for the time being
      Stalling one SRU will cause its BUSY/VETO flag to stay high after the event
      which also stalls the remaining SRUs. Thus, one register write to a single SRU
      is sufficient to control all SRUs.
    */
      istat = sruWriteRegister(srus->controlsocket,&srus->sruserver,SRU_REG_ALLOWED_EVENTS,busyon);
    }
    // Send a software trigger - if required
    if (software_trigger != 0) {
      istat = 0;
      // TEST comment the two lines below printf
      //printf("Send software trigger %d\n",evtreceived);
      usleep(delay);
      istat = sruWriteRegister(srus->controlsocket,&srus->sruserver,SRU_REG_STRIG_CMD,0);
      // END TEST
      if (istat != 0) {
	sprintf(msgbuf, "%s: sruWriteRegister returned %d\n",__FUNCTION__,istat);
	printf("%s",msgbuf);
	return -1;
      }
    }
    newevent = 1;
    evtpackets = 0;
    // poll for data - first is always a new event, wait forever
    timeout = -1;
    while(newevent) {
      //     printf("%s entered recv %d nbevents %d\n",__FUNCTION__,evtreceived,srus->nbevents);
      runconnect[0].revents = 0;
      runconnect[1].revents = 0;
      istat = poll(runconnect,2,timeout);
      if (istat == 0) {// timedout
	sprintf(msgbuf, "%s: poll timeout event received %d\n",__FUNCTION__,evtreceived);
	printf("%s",msgbuf);
	newevent = 0; //signal end of event
      }
      else if (istat > 0) { //returned event
	if ((runconnect[1].revents & POLLIN) != 0) { //SIGNAL
	  if (read(runconnect[1].fd, &siginfo, sizeof(siginfo)) != sizeof(siginfo)) {
	    sprintf(msgbuf, "%s: signalfd poll error %s\n",__FUNCTION__,strerror(errno));
	    printf("%s",msgbuf);
	  }
	  printf("%s: Exiting via %s\n", __FUNCTION__,strsignal(siginfo.ssi_signo));
	  return(-3);
	}
	if ((runconnect[0].revents & POLLIN) != 0) { // data from SRU - event data
	  if (evtpackets == 0) evtreceived++; // increase events received
	  evtpackets++; // count udp packets received
	  evtheader = (unsigned int *)evtbuffer;
	  data_buffer = (unsigned int *)evtbuffer;
	  data_buffer += (EVTHEADER_LENGTH + 1);
	  data_length = 0;
	  total_length = EVTHEADER_LENGTH;
	  r_length = recvfrom(runconnect[0].fd, data_buffer, BUFLEN, 0, (struct sockaddr *) &s_sru, &s_length);

	  //	  printf("evt %d packet %d length %d\n",evtreceived,evtpackets,r_length);
	  
	  if (r_length > 0) {
	    data_length = r_length >> 2; // 32 bit words
	    // check for SRU header 10
	    // check for SRU trailer 11
	    total_length += data_length;
	    evtheader[0] = total_length; // exklusive length in 32 bit words
	    evtheader[1] = EVTHEADER_LENGTH - 1; // exklusive length of event header in 32 bit words
	    evtheader[2] = 0x22222222; // event identifier
	    sscanf(inet_ntoa(s_sru.sin_addr),"%d.%d.%d.%d",&v4,&v3,&v2,&v1);
	    ipaddress = (v4 << 24) | (v3 << 16) | (v2 << 8) | v1;
	    evtheader[3] = ipaddress; // SRU identifier
	    printf("%s: Event %d packet %d length %d from %s (%08X) port %d\n",__FUNCTION__,
		   evtreceived,evtpackets,r_length,inet_ntoa(s_sru.sin_addr),ipaddress,ntohs(s_sru.sin_port));
	    fflush(stdout);
	    if (datalogging) {
	      fwrite(evtbuffer,(total_length + 1) << 2,1,dout);
	    }
	    if ((data_buffer[data_length-1] & 0xC0000000) == 0xC0000000) {
	      // TEST uncomment
	      //	      printf("END OF EVENT %08X\n",data_buffer[data_length-1]);
	      newevent = 0;
	    }
	    else timeout = 20; // wait < 20ms for next udp packet
	  }
	  else if (r_length < 0) {
	    sprintf(msgbuf,"%s: length %d recvfrom error %s\n",__FUNCTION__,r_length,strerror(errno));
	    printf("%s",msgbuf);
	  }
	}
	if ((runconnect[0].revents & POLLHUP) != 0) { // network hangup - close all
	  sprintf(msgbuf, "%s: socket hangup\n",__FUNCTION__);
	  printf("%s",msgbuf);
	  return -1;   
	}
      }
      else { // returned an error - close all
	sprintf(msgbuf, "%s: socket poll error %s\n",__FUNCTION__,strerror(errno));
	printf("%s",msgbuf);
	return -2;   
      }
    }
  }
  // reset to "infinite triggers to accept
  if (busyon != 0) {
    istat = sruWriteRegister(srus->controlsocket,&srus->sruserver,SRU_REG_ALLOWED_EVENTS,0xFFFFFFFF);
  }
  return 0;
}


int main(int argc, char **argv)
{
  int errst;
  errst = decodeParameters(argc,argv);
  if (errst != 0) {
     return(-1);
  }

  strcpy(srus.sruserver.controlip,SRU_CONTROL_IP);
  srus.sruserver.controlport = SRU_CONTROL_PORT;
  srus.listenport = SRU_LISTEN_PORT;
  strcpy(srus.sruserver.readoutip,SRU_READOUT_IP);
  srus.sruserver.readoutport = SRU_READOUT_PORT;
  strcpy(srus.localip,SRU_LOCAL_IP);

  sigemptyset(&sigset);
  sigaddset(&sigset, SIGTERM); // kill -15
  sigaddset(&sigset, SIGQUIT); // "CTRL-\"
  sigaddset(&sigset, SIGINT); // "CTRL-C"
  sigprocmask(SIG_SETMASK, &sigset, NULL);

  signal_fd = signalfd(-1, &sigset, 0);
  if (signal_fd == -1) {
    printf("%s: signalfd error %s\n",argv[0],strerror(errno));
    return(-1);
  }
  runconnect[0].fd = 0;
  runconnect[0].events = 0;
  runconnect[1].fd = signal_fd;
  runconnect[1].events = POLLIN;
  
  
  srus.sruserver.dtcrdomask1 = ~srus.sruserver.dtcrdomask1 & 0xFFFFF;
  srus.sruserver.dtcrdomask2 = ~srus.sruserver.dtcrdomask2 & 0xFFFFF;
  strcpy(srus.interface,sruif);
  printf("Run with\n--------\nEvents: %d\n",nbevents);
  printf("Datalogging: %d\n",datalogging);
  if (datalogging != 0) printf("Data logfile: %s\n",datafile);
  printf("Softtrigger: %d\n",software_trigger);
  if (software_trigger != 0)printf("Rate: %d\n",rate);
  printf("SRU interface: %s\n",srus.interface);
  printf("SRU local IP: %s\n",srus.localip);
  printf("DTCmask2 and DTCmask1: %08X %08X\n",srus.sruserver.dtcrdomask2,srus.sruserver.dtcrdomask1);
  printf("Running with busyonflag = %d\n",busyon);
  puts("--------");

  // HERE FIND MY IP ON GIVEN INTERFACE
  if (set_sruif != 0) {
    errst = findip(srus.interface,srus.localip,srus.broadcastip);
    if (errst != 0) close(signal_fd);
    if (errst > 0) {
      printf("ERR: %s findip error %s\n",argv[0],strerror(errst));
      close(signal_fd);
      return(1);
    }
    else if (errst == -1) {
      printf("ERR: %s findip %s interface not found\n",argv[0],srus.interface);
      close(signal_fd);
      return(1);
    }
    else if (errst != 0) {
      printf("ERR: %s findip multiple %s interface found %d\n",argv[0],srus.interface,-(errst+1));
      return(1);
    }
    printf("\t  Interface: <%s>\n",srus.interface);
    printf("\t  Address : <%s>\n",srus.localip); 
    printf("\t  broadcast : <%s>\n",srus.broadcastip);
  }
  // TEST uncomment
  /* 
  errst = dataSruConnect(&srus);
  if (errst != 0) {
    printf("%s: dataSruConnect returned with error %d\n",argv[0],errst);    
    close(signal_fd);
    close(srus.controlsocket);
    return(-3);
  }
  if (datalogging != 0) {
    dout = fopen(datafile,"w");
  }
 dataSruReceiver(&srus);
  close(signal_fd);
  close(srus.readoutsocket);
  close(srus.controlsocket);
  if (dout != NULL) fclose(dout);
  return 0;
  */
 // END TEST
  errst = scSruConnect(&srus);
  if (errst != 0) {
    close(signal_fd);
    printf("%s: scSruConnect returned with error %d\n",argv[0],errst);
    return(-2);
  }

  errst = sruLoadReadoutList(srus.controlsocket,&srus.sruserver);
  if (errst != 0) {
    close(signal_fd);
    close(srus.controlsocket);
    printf("%s:  sruLoadReadoutList returned with error %d\n",argv[0],errst);
    return(-6);
  }

  
  // We clear all counters before a run
  /*
Bit 3: Clear Bunch counter (som vi antagligen inte använder, eller hur?)
Bit 2: Clear all trigger counters (L0CNT, L1CNT, L2ACNT, L2RCNT, RDOCNT, osv)
Bit 1: Clear event counter
Bit 0: Clear timestamp. Will start counting on next trigger event.
  */
  errst = sruWriteRegister(srus.controlsocket,&srus.sruserver,SRU_REG_TRIG_CNT_CLR,0xF);
  if (errst != 0) {
    printf("%s:sruWriteRegister SRU_REG_TRIG_CNT_CLR returned with error %d\n",argv[0],errst);    
  }

  
  errst = dataSruConnect(&srus);
  if (errst != 0) {
    printf("%s: dataSruConnect returned with error %d\n",argv[0],errst);    
    close(signal_fd);
    close(srus.controlsocket);
    return(-3);
  }
  
  
  // HERE WRITE LOCAL IP TO SRU REGISTER
  if (set_sruip != 0) {
    unsigned int v4,v3,v2,v1,valuewr;
    sscanf(srus.localip,"%d.%d.%d.%d",&v4,&v3,&v2,&v1);
    valuewr = (v4 << 24) | (v3 << 16) | (v2 << 8) | v1;
    errst = sruWriteRegister(srus.controlsocket,&srus.sruserver,SRU_REG_READOUT_IP,valuewr);
    if (errst != 0) {
      printf("%s: could not write local ip %s to SRU returned %d\n",argv[0],srus.localip,errst);
      printf("%s: we ignore it for now\n",argv[0]);
    }
  }
  // Data handler
  srus.nbevents = nbevents;
  dout = NULL;
  if (datalogging != 0) {
    dout = fopen(datafile,"w");
    if (dout == NULL) {
      printf("%s: could not open datafile %s\n",argv[0],datafile);    
      close(signal_fd);
      close(srus.readoutsocket);
      close(srus.controlsocket);
      return(-4);
    }
  }
  
  errst = dataSruReceiver(&srus);
  if (errst != 0) {
    printf("%s: dataSruReceiver returned with error %d\n",argv[0],errst);    
    close(signal_fd);
    close(srus.readoutsocket);
    close(srus.controlsocket);
    if (dout != NULL) fclose(dout);
    return(-5);
  }
  close(signal_fd);
  close(srus.readoutsocket);
  close(srus.controlsocket);
  if (dout != NULL) fclose(dout);
  return 0;
}