/*
Compile:
  gcc -Wall -o saltroaddrtest saltroaddrtest.c

Run:
./saltrowrrdreg <options>

Options:
--fec <fec> DTC link front end card" (default 0)
--device <device> device to use
--loop <number of loops, 0 = infinity> (default 1)

csvfile:
Adddress,Send,Addr0,Addr1,Addr2,Addr3,Addr4,Addr5,Addr6,Addr7,Errorcode
*/
#include <getopt.h>
#include <fcntl.h>
#include <signal.h>
#include <stdio.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <sys/stat.h>
#include <errno.h>
#include <stdlib.h>

#define FEC 0
#define CHIP 0
#define CHANNEL 0
#define DEVICE "/dev/chipaddress_1"

//#define LOCAL_IP "130.235.185.193"
//#define SRU_IP "130.235.185.193"
#define LOCAL_IP "192.168.4.11"
#define SRU_IP "192.168.4.4"

#define SRU_WAIT_WRITE 500


unsigned int  valuerd,valuewr;
int fec;
int loop;
int chip;
int signal_stop = 0;
int logdata = 0;
char device[128];
char csvfile[256];

typedef struct {
  char controlip[64];
  int controlport;
  struct sockaddr_in si_sru;
  int scsocket; // socket for slow control
  int listenport;
  char readoutip[64];
  unsigned int readoutipnb;
  int status; // status from configfile
  int linkup; // it it answered ping
  int active; // status & linkup is set and FMVER could be read
} SRUSERVER;


typedef struct {
  struct sockaddr_in si_controlme;
  int controlsocket; // socket for control
  int listenport; // port for listen on control
  int nbrdpackets; // Number of packets expected back
  SRUSERVER sruserver;
} SRU;


typedef struct {
  int iret[8];
  int adevl;
  int send;
  unsigned int ErrorCode;
} CHIPS;
CHIPS chips[8];

int sendcommand(int devfd, char cmd);
int scSruConnect(SRU *srus);
int saltroWriteRegister(int handle,SRUSERVER *srv,int broadcast, int node, int chip, int channel, int reg, unsigned int value);
int saltroReadRegister(int handle, SRUSERVER *srv, int node, int chip, int channel, int reg, unsigned int *value);
void signal_handler(int signum);
int decodeParameters(int argc, char **argv);
int makeCsvfile();


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

  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(LOCAL_IP, &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: bind error %s\n",__FUNCTION__,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;
}


/* SALTRO part */
int saltroWriteRegister(int handle,SRUSERVER *srv,int broadcast, int node, int chip, int channel, int reg, unsigned int value)
{
  unsigned int buf[4];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  unsigned int myreg;


  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
  }
  myreg = (broadcast << 18) | (node << 12) | (chip << 9) | (channel << 5) | reg;
  buf[2] = htonl(myreg | 0x40000000);
  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;
}

int saltroReadRegister(int handle, SRUSERVER *srv, int node, int chip, int channel, 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);
  unsigned int myreg,mynode;
  unsigned int myregrd;
  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
 }

  myregrd = (node << 12) | (chip << 9) | (channel << 5) | reg;
  buf[2] = htonl(myregrd | 0xC0000000);
  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) {
    //    perror("-3");
    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 illegal packet (lengh = %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 != node) {
    printf("%s: Received not expected node %08X expected %08X\n",
	   __FUNCTION__,mynode,node);
    return -6;
  }
  myreg = ntohl(buf[1]);
  if (myreg != (myregrd | 0xC0000000)) {
    printf("%s: Received not expected register %08X expected %08X\n",
	   __FUNCTION__,myreg,myregrd | 0xC0000000);
    return -7;
  }
  *value = ntohl(buf[2]);

  return 0;
}

int makeCsvfile()
{
  FILE *fcsv;
    // Make ErrorCode
    for (int j = 0; j < 8; j++) {
      if (chips[j].adevl != 0)  chips[j].ErrorCode |= 1;
      if (chips[j].send != 1) chips[j].ErrorCode |= 2;
      if (chips[j].iret[j] != 0) chips[j].ErrorCode |= 4;
      for (int k = 0; k < 8; k++) {
	if (j != k) {
	  if ( chips[j].iret[k] != loop) chips[j].ErrorCode |= (8 << k);
	}
      }
    }
    if (logdata == 0) {
      printf("Adddress,Adevl,Send,");
      for (int chip = 0; chip < 8; chip++) 
	printf("Addr%d,",chip);
      printf("Errorcode\n");
      for (int j = 0; j < 8; j++) {
	printf("%d,%d,%d,",j,chips[j].adevl,chips[j].send);
	for (int k = 0; k < 8; k++) {
	  printf("%d,",chips[j].iret[k]);
	}
	printf("%X\n",chips[j].ErrorCode);
      }
    }
    else {
      struct stat   fileBuffer;
      int csvexist = 0;
      if (stat (csvfile, &fileBuffer) == 0) csvexist = 1;
      fcsv = fopen(csvfile,"a");
      if (fcsv != NULL) {
	if (csvexist == 0) {
	  fprintf(fcsv,"Adddress,Adevl,Send,");
	  for (int chip = 0; chip < 8; chip++) 
	    fprintf(fcsv,"Addr%d,",chip);
	  fprintf(fcsv,"Errorcode\n");
	}
	for (int j = 0; j < 8; j++) {
	  fprintf(fcsv,"%d,%d,%d,",j,chips[j].adevl,chips[j].send);
	  for (int k = 0; k < 8; k++) {
	    fprintf(fcsv,"%d,",chips[j].iret[k]);
	  }
	  fprintf(fcsv,"%X\n",chips[j].ErrorCode);
	}
      }
      else {
	printf("FILE: %s not opened\n",csvfile);
	return (1);
      }
    }
    return 0;
}

void signal_handler(int signum)
{
  switch(signum) {
  case SIGQUIT:
  case SIGTERM:
  case SIGINT:
    puts("\nSIGTERM");
    signal_stop = 1; break;
    /*
  case SIGTERM:
  case SIGQUIT: break;
    */
  }
}

int decodeParameters(int argc, char **argv)
{
  int c;
  int option_index = 0;
  char *short_options = "c:d:f:l:";
  const struct option long_options[]=
  {
   {"csvfile",1,0,'c'},
   {"device",1,0,'d'},
   {"fec",1,0,'f'},
   {"loop",1,0,'l'},
   {0, 0, 0, 0}
  };
  fec = FEC;
  loop = 1;
  strcpy(device,DEVICE);
  csvfile[0] = '\0';
  if (argc > 1)
  {
    while ((c = getopt_long(argc, argv, short_options,
                                        long_options, &option_index)) != EOF)
    {
      switch (c)
      {
      case 'c': strcpy(csvfile,optarg); logdata=1; break;
      case 'd': strcpy(device,optarg) ; break;
      case 'f': sscanf(optarg,"%d",&fec); break;
      case 'l': sscanf(optarg,"%d",&loop); break;
      default: printf("Unkown paramater\n"); return 1;
      }
    }
  }
  else {
    printf("Options: \n \
	   --device <device> USB port (default %s)\n \
	   --fec <fec> 0-39 DTC link front end card (default 0)\n \
	   --loop <number of loops, 0 = infinity> (default 1)\n \
	 ",DEVICE);
    return 1;
  }
  return 0;
}

int sendcommand(int devfd, char cmd)
{
  int istat;
  istat = write(devfd,&cmd,1);
  if (istat < 0 ) {
    printf("WRITE returned %d error %s\n",istat,strerror(errno));
    return (4);
  }
  return 0;
}

int main(int argc, char **argv){
  int devfd;
  SRU srus;
  int iret;;
  
  int errst;

    signal(SIGINT,signal_handler);
    signal(SIGQUIT,signal_handler);
    signal(SIGTERM,signal_handler);

    strcpy(srus.sruserver.controlip,SRU_IP);
    srus.sruserver.controlport = 4097;
    srus.listenport = 4100;

    if (decodeParameters(argc,argv)) return 0;
    printf("Parameters: FEC= %d  LOOP %d DEVICE %s FILE %s\n", fec,loop,device,csvfile);

    errst = scSruConnect(&srus);
    if (errst != 0) return(1);

    devfd = open(device,O_RDWR | O_NONBLOCK);
    if (devfd < 0 ) {
      printf("OPEN returned %d error %s\n",devfd,strerror(errno));
      return (3);
    }
    
    memset(chips,0,sizeof(chips));
    // Read ADEVL bits 15-8 = Chipaddress
    for (int j = 0; j < 8; j++) {
      iret = sendcommand(devfd,0x00+j);
      if (iret == 0) chips[j].send++;
      for (int k = 0; k < loop; k++) {
	for (int l = 0; l < 8; l++) {
	  valuerd=0xF00;
	  iret = saltroReadRegister(srus.controlsocket,&srus.sruserver,fec,l,0,0x11,&valuerd);
	  //	  printf("CHIP %d READ %d IRET %d VALUE %X\n",l,l,iret,valuerd);
	  if (iret != 0) {
	    chips[j].iret[l]++;
	  }
	  else if (l == j) {
	    if (((valuerd & 0xFF00) >> 8) != j) chips[j].adevl++;
	  }
	}
      }
      if (signal_stop) break;
    }
    iret = makeCsvfile();
    close(srus.controlsocket);
    close(devfd);
    if (iret != 0) return 1;
    return 0;
}