/*
  Source code: sru-init.c

  Compile with:
  gcc -Wall -o sru-init sru-init.c

  Program: sru-udpserver
  Arguments:
  --dest, -d IP addres of SRU
  --file, -f filename to output binary data to
  --init, -i send connect to SRU
  --output, -o output decoded data on standard output
  --port, -p: port to listen on


  i.e. to start and send connect to SRU, log data on file and
  output decoded data on screen

  sru-udpserver  -f dummy.dat -i -o

*/

#include <stdlib.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <getopt.h>
#include <sys/stat.h>
#include <fcntl.h>


#define BUFLEN 5000000
#define NPACK 10
#define PORT 6007
#define READOUT_PORT 6006
#define SRU_IP "192.168.4.5"

int s_sockfd; // Socket descriptor
int d_sockfd; // Socket descriptor
int ro_sockfd; // readout socket
int s_port = PORT; // port to use
char ip_dest[20]; // IP address of destination
int d_port = PORT; // port to use
int readout_port = READOUT_PORT; // listen for data
int r_event = 0;

int sru_init_com = 0;
FILE *fout;
char logfile[128];
int sru_log_data = 0;
int sru_decode_data = 0;

void signal_handler(int signum);
int getParameters(int argc, char **argv);
void die(char *s);
void dumpBuffer(unsigned char *buf, int length);
void decodeSaltroData(unsigned char *buf,int length);


void die(char *s)
{
  perror(s);
  printf("\n%s WE EXIT!\n",__FUNCTION__);
  close(s_sockfd);
  close(ro_sockfd);
  close(d_sockfd);
  exit(1);
}

void signal_handler(int signum)
{
  switch(signum) {
    // close connections of active connections

  case SIGINT:
  case SIGTERM:
  case SIGQUIT:
    fprintf(stderr,"\n%s: CLOSE SOCKET\n",__FUNCTION__);
    printf("Packets received %d\n",r_event);
    close(s_sockfd);
    close(d_sockfd);
    close(ro_sockfd);
    if (sru_log_data != 0) fclose(fout);
    exit (1);
    break;
  }
}

void dumpBuffer(unsigned char *buf, int length)
{
  int i,j;
  unsigned int value;
  printf("#DATA:\n");
  for (j = 0, i = 1; j < length; j+=4,i++) {
    value = 0;
    value |= (buf[j+3] << 24);
    value |= (buf[j+2] << 16);
    value |= (buf[j+1] << 8);
    value |= buf[j+0];
    printf("%08X ",value);
    if ( (i % 8) == 0) putchar('\n');
  }
  if ((i % 8) != 1) putchar('\n');
}

void decodeSaltroData(unsigned char *buf,int length)
{
  int nbwords,j;
  unsigned int *value;
  value = (unsigned int *) buf;

  for (j = 0; j < length; j+=4) {
    if ((*value & 0xC0000000) == 0x40000000) {
      printf("\nHeader: %08X Board Address: %d  Chip Address: %d Saltro Channel: %d Number of words: %d\n",
             *value,(*value >> 7) & 0x1FF, (*value >> 4) & 0x7, *value & 0xF, (*value >> 16) & 0x3FF);
      nbwords = (*value >> 16) & 0x3FF;
    }
    else if ((*value & 0xC0000000) == 0x00000000) {
      printf("Data %3d: %4d[0x%03X]    Data %3d: %4d[0x%03X]    Data %3d: %4d[0x%03X]\n",
             nbwords,(*value >> 20) & 0x3FF,(*value >> 20) & 0x3FF,
             nbwords-1,(*value >> 10) & 0x3FF,(*value >> 10) & 0x3FF,
             nbwords-2,*value & 0x3FF,*value & 0x3FF);
      nbwords -= 3;
    }
    else {
      printf("Unknown header bits %08X\n",*value);
      return;
    }
    value++;
  }
}

int getParameters(int argc, char **argv)
{
  char *short_options = "d:if:op:";
  int option_index = 0;
  int c;
  const struct option long_options[]=
    {
      {"dest",1,0,'d'},
      {"file",1,0,'f'},
      {"init",0,0,'i'},
      {"output",0,0,'o'},
      {"port",1,0,'p'},
    };

  strcpy(ip_dest,SRU_IP);

  if (argc > 1)
    {
      while ((c = getopt_long(argc, argv, short_options,
                              long_options, &option_index)) != EOF)
        {
          switch (c)
            {
            case 'd': strncpy(ip_dest,optarg,sizeof(ip_dest));
              break;
            case 'f':
              strncpy(logfile,optarg,sizeof(logfile));
              sru_log_data = 1;
              break;
            case 'i': sru_init_com = 1; break;
            case 'o': sru_decode_data = 1; break;
            case 'p': sscanf(optarg,"%d",&s_port);
              break;
            default:
              printf("%s: Unknown option %s\n",__FUNCTION__,argv[optind-1]);
              return 1;
            }
        }
    }
  return 0;
}

int main(int argc, char **argv)
{
  // Destination SRU
  struct sockaddr_in d_sru;
  socklen_t d_length=sizeof(d_sru);

  // Source socket from SRU
  struct sockaddr_in s_listener, s_sru;
  socklen_t s_length=sizeof(s_sru);

  // Source socket from SRU
  struct sockaddr_in readout_sru;
  // socklen_t ro_length=sizeof(readout_sru);


  // Set socket timeout
  struct timeval tv;
  tv.tv_sec = 0;
  tv.tv_usec = 100000;

  int r_length;
  int r_event;
  unsigned char buf[BUFLEN];

  if (getParameters(argc,argv) != 0) exit(1);

  printf("#Listen on port: %d\n#sru ip %s\n#sru port %d\n",s_port,ip_dest,d_port);

  if ((d_sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) die("D_SOCKET");
  memset((char *) &d_sru, 0, sizeof(d_sru));
  d_sru.sin_family = AF_INET;
  d_sru.sin_port = htons(d_port);
  if (inet_aton(ip_dest, &d_sru.sin_addr)==0) {
    fprintf(stderr, "inet_aton() failed\n");
    exit(1);
  }


  // Create an UDP socket (really IPPROTO_UDP can also be set to 0)
  // if it fails we die
  if ((s_sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) die("S_SOCKET");

  memset((char *) &s_listener, 0, sizeof(s_listener));
  s_listener.sin_family = AF_INET;
  s_listener.sin_port = htons(s_port);
  // Set to listen on all local interfaces - may change to one specific interface
  // in the case we use a specific internal network, e.g.:
  // = inet_aton("192.168.0.1")
  s_listener.sin_addr.s_addr = htonl(INADDR_ANY);
  //assign the address specified to by s_listener to the socket  referred  to
  //by  the  file  descriptor  s_sockfd
  if (bind(s_sockfd, (struct sockaddr *)&s_listener, sizeof(s_listener))==-1)
    die("BIND");
  // Timeout
  if (setsockopt(s_sockfd, SOL_SOCKET, SO_RCVTIMEO,&tv,sizeof(tv)) < 0) {
    perror("Error");
  }

  // Create an UDP socket (really IPPROTO_UDP can also be set to 0)
  // if it fails we die
  if ((ro_sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) die("S_SOCKET");

  memset((char *) &readout_sru, 0, sizeof(readout_sru));
  readout_sru.sin_family = AF_INET;
  readout_sru.sin_port = htons(readout_port);
  // Set to listen on all local interfaces - may change to one specific interface
  // in the case we use a specific internal network, e.g.:
  // = inet_aton("192.168.0.1")
  readout_sru.sin_addr.s_addr = htonl(INADDR_ANY);
  //assign the address specified to by s_listener to the socket  referred  to
  //by  the  file  descriptor  s_sockfd
  if (bind(ro_sockfd, (struct sockaddr *)&readout_sru, sizeof(readout_sru))==-1)
    die("BIND");



  // Add signal handler to cleanup things when killed
  signal(SIGINT,  signal_handler);
  signal(SIGTERM, signal_handler);
  signal(SIGQUIT, signal_handler);

  // Contact the SRU - if required
  if (sru_init_com != 0) {
    buf[0] = 0;
    buf[1] = 0;
    buf[2] = 0;
    buf[3] = 2; // What does this mean?
    printf("Sending buffer to connect %02X%02X%02X%02X\n",buf[0],buf[1],buf[2],buf[3]);
    if ( (r_length = sendto(d_sockfd, buf, 4, 0, (struct sockaddr *) &d_sru, d_length)) ==-1) die("D_SENDTO");
  }
  // infinite loop waiting for data using recvfrom()
  // may be changed to use poll() as well, to be able to listen to more sources
  r_event = 0;
   while (r_event == 0) {
    if ((r_length = recvfrom(s_sockfd, buf, BUFLEN, 0, (struct sockaddr *) &s_sru, &s_length))==-1)
      die("RECVFROM");
      sleep(1);
    r_event++;
    printf("#REPLY %d length %d from %s:%d\n",r_event,r_length,
    inet_ntoa(s_sru.sin_addr), ntohs(s_sru.sin_port));
    dumpBuffer(buf,r_length);
    dumpBuffer(buf,4);
  }


  // we should never arrive here!
  close(s_sockfd);
  close(ro_sockfd);
  close(d_sockfd);
  return 1;
}