/*
* This file is part of OpenKC
*
* Copyright(c) Matthias Schöpfer (mschoepf@techfak.uni-bielefeld.de)
* http://opensource.cit-ec.de/projects/openkc
*
* This file may be licensed under the terms of of the
* GNU Lesser General Public License Version 3 (the ``LGPL''),
* or (at your option) any later version.
*
* Software distributed under the License is distributed
* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
* express or implied. See the LGPL for the specific language
* governing rights and limitations.
*
* You should have received a copy of the LGPL along with this
* program. If not, go to http://www.gnu.org/licenses/lgpl.html
* or write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The development of this software was supported by the
* Excellence Cluster EXC 277 Cognitive Interaction Technology.
* The Excellence Cluster EXC 277 is a grant of the Deutsche
* Forschungsgemeinschaft (DFG) in the context of the German
* Excellence Initiative.
*
*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <time.h>
#include <arpa/inet.h>
#include <sys/time.h>
#include <sys/select.h>
#include <pthread.h>
#include <signal.h>
#include <sched.h>
#include <math.h>

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "fri_okc_types.h"
#include "fri_okc_helper.h"
#include <fricomm.h>
#include "fri_okc_hostconfig.h"


#ifdef HAVE_GETOPT_H
#include <getopt.h>
#endif





#define DEFAULTDELAY 12000000

#define SPO_QUIET 1
#define OPS_SIGNAL_PLANNER_READY 0x04

volatile sig_atomic_t keep_going = 1;
volatile sig_atomic_t late = 0;

typedef struct SERVER_PING_OPTS_T
{
  char* hostname;
  unsigned short port;
  unsigned long int delay_in_nanoseconds;
  int sockfd;
  int flags;
  pthread_t ping_thread;
  pthread_mutex_t wait_on_next_ping_mx;
  pthread_cond_t wait_on_next_ping_cd;
  timer_t *wake_ping_timer;
} server_ping_opts_t;



static void
_print_help (const char* progname)
{
#ifdef HAVE_GETOPT_LONG
  printf ("%s Version %s\nCopyright by Matthias Sch\"opfer "
	  "%s\n\n",progname,VERSION,PACKAGE_BUGREPORT);
  printf ("This is free software; see the source for copying conditions."
	  "There is NO \nwarranty; not even for MERCHANTABILITY or FITNESS FOR A"
	  " PARTICULAR PURPOSE.\n"
	  "\nThe development of this software was supported by the Excellence"
	  " Cluster EXC\n"
	  "277 Cognitive Interaction Technology. The Excellence Cluster EXC "
	  "277 is a grant\nof the Deutsche Forschungsgemeinschaft (DFG) in the "
	  "context of the German\nExcellence Initiative.\n\n");
  printf ("USAGE: %s [OPTIONS]\n",progname);
  printf ("--help/-h\t\t\t = print this help\n");
  printf ("--quiet/-q\t\t\t = be really quiet\n");
  printf ("--port/-p <port>\t = set port address (default: %s)\n",OKC_PORT);
  printf ("--delay_ns/-d <delay in nanoseconds>\t\t = set delay between packets"
	  "(default: %d)\n",DEFAULTDELAY);
  printf ("--server/-s=<fqdn>\t\t = connect to server at fqdn (default: %s)\n",
	  OKC_HOST);
#else
  printf ("%s Version %s\nCopyright by Matthias Sch\"opfer "
	  "%s\n\n",progname,VERSION,PACKAGE_BUGREPORT);
  printf ("This is free software; see the source for copying conditions."
	  "There is NO \nwarranty; not even for MERCHANTABILITY or FITNESS FOR A"
	  " PARTICULAR PURPOSE.\n"
	  "\nThe development of this software was supported by the Excellence"
	  " Cluster EXC\n"
	  "277 Cognitive Interaction Technology. The Excellence Cluster EXC "
	  "277 is a grant\nof the Deutsche Forschungsgemeinschaft (DFG) in the "
	  "context of the German\nExcellence Initiative.\n\n");
  printf ("USAGE: %s [OPTIONS]\n",progname);
  printf ("-h\t\t = print this help\n");
  printf ("-q\t\t = be really quiet\n");
  printf ("-p <port> = set port address (default: %d)\n",OKC_PORT);
  printf ("-d <delay in nanoseconds>\t\t = set delay between packets"
	  "(default: %d)\n",DEFAULTDELAY);
  printf ("-s=<fqdn>\t\t = connect to server at fqdn (default: %s)\n",
	  OKC_HOST);
#endif
}

void
catch_interrupt (int sig)
{
  keep_going = 0;
  signal (sig, catch_interrupt);
  
}


/* get sockaddr, IPv4 or IPv6:*/
static void 
*get_in_addr(struct sockaddr *sa)
{
    if (sa->sa_family == AF_INET) {
        return &(((struct sockaddr_in*)sa)->sin_addr);
    }

    return &(((struct sockaddr_in6*)sa)->sin6_addr);
}

static void
fill_spo_defaults (server_ping_opts_t* spo)
{
  spo->hostname = malloc (strlen (OKC_HOST)+1);
  
  if (NULL == spo->hostname)
    {
      perror ("malloc");
      exit (EXIT_FAILURE);
    }

  strcpy (spo->hostname,OKC_HOST);

  spo->port = atoi(OKC_PORT);
  spo->delay_in_nanoseconds = DEFAULTDELAY;
  spo->flags = 0;
}

static void
init_server_ping_opts (server_ping_opts_t* spo)
{
  spo->wake_ping_timer = malloc (sizeof (timer_t));
  if (spo->wake_ping_timer == NULL)
    {
      perror ("okc-ping-server: init_server_ping_opts: malloc");
      exit (EXIT_FAILURE);
    }

  if (pthread_mutex_init (&spo->wait_on_next_ping_mx, NULL) != 0)
    {
      perror ("okc-ping-server: init_server_ping_opts: pthread_mutex_init");
      exit (EXIT_FAILURE);
    }

  if (pthread_cond_init (&spo->wait_on_next_ping_cd, NULL) != 0)
    {
      perror ("okc-ping-server: init_server_ping_opts: pthread_cond_init");
      exit (EXIT_FAILURE);
    }
  spo->sockfd = -1;
}
  
static server_ping_opts_t*
parse_args (int argc, char** argv)
{
#ifdef HAVE_GETOPT_LONG
  static struct option long_options[] = {
    {"help", no_argument, 0, 'h'},
    {"quiet", no_argument, 0, 'q'},
    {"port", required_argument, 0, 'p'},
    {"delay", required_argument, 0, 'd'},
    {"server",required_argument,0,'s'},
    {0, 0, 0, 0}
  };
#endif

  char* optstring = "hqp:d:s:";
  char* progname;
  
  int ret; 
  
#ifdef HAVE_GETOPT_LONG
  int longindex;
#endif
  server_ping_opts_t* spo;

  spo = malloc (sizeof (server_ping_opts_t));

  if (NULL == spo)
    {
      perror ("malloc");
      exit (EXIT_FAILURE);
    }

  init_server_ping_opts (spo);

  progname = malloc (strlen (argv[0])+1);
  if (NULL == progname)
    {
      perror ("malloc");
      exit (EXIT_FAILURE);
    }

  strcpy (progname,argv[0]);
  

  fill_spo_defaults (spo);

  #ifdef  HAVE_GETOPT_LONG 
  while ((ret = getopt_long_only (argc, argv, optstring, long_options, 
				  &longindex)) != -1)
#else
    while ((ret = getopt (argc,argv,optstring)) != -1)
#endif
    {
      if (ret == '?' || ret == 'h')
	{
	  _print_help (progname);
	  exit (EXIT_FAILURE);
	}
      switch (ret)
	{
	case 'q':
	  spo->flags |= SPO_QUIET;
	  break;
	case 'p':
	  if (optarg != NULL)
	    spo->port = atoi(optarg);
	  else
	    {
	      printf ("Option port requires an argument!\n");
	      _print_help (progname);
	      exit (EXIT_FAILURE);
	    }
	  break;
	case 'd':
	  if (optarg != NULL)
	    spo->delay_in_nanoseconds = atoi (optarg);
	   else
	    {
	      printf ("Option delay requires an argument!\n");
	      _print_help (progname);
	      exit (EXIT_FAILURE);
	    }
	  break;
	case 's':	 
	  if (optarg != NULL)
	    {
	      free (spo->hostname);
	      spo->hostname = malloc (strlen (optarg)+1);
	      if (NULL == spo->hostname)
		{
		  perror ("malloc");
		  exit (EXIT_FAILURE);
		}
	      strcpy (spo->hostname,optarg);
	    }
	  else
	    {
	      printf ("Option server requires an argument!\n");
	      _print_help (progname);
	      exit (EXIT_FAILURE);
	    }
	  break;
	default:
	  _print_help (progname);
	  exit (EXIT_FAILURE);
	}
    }
  
  free(progname);
  return spo;
}

int
connect_to_server (server_ping_opts_t* spo)
{
  int sockfd;
  struct addrinfo hints, *servinfo, *p;
  char s[INET6_ADDRSTRLEN];
  int rv;
  int yes=1;

  char portnumber[64];

  snprintf (portnumber,64,"%u",spo->port);
  
  memset(&hints, 0, sizeof hints);
  hints.ai_family = AF_INET;
  hints.ai_socktype = SOCK_DGRAM;

  fprintf (stderr,"hostname = %s\n",spo->hostname);
  
  if ((rv = getaddrinfo(OKC_HOST,OKC_PORT, &hints, &servinfo)) != 0) 
    {
      fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
      exit (EXIT_FAILURE);
    }
  
  /* loop through all the results and connect to the first we can */
  for(p = servinfo; p != NULL; p = p->ai_next) 
    {
      if ((sockfd = socket(p->ai_family, p->ai_socktype,
			   p->ai_protocol)) == -1) 
	{
	  if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	    perror("socket");
	  continue;
	}

      if (0 != setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int)))
	{
	  perror ("setsockopt: SO_REUSADDR");
	  
	}

      if (bind (sockfd, p->ai_addr, p->ai_addrlen) < 0) 
	{
	  perror ("bind");
	  close(sockfd);
	  continue;
	}
      break;
    }
  
  if (p == NULL) 
    {
      fprintf(stderr, "failed to connect\n");
      exit (EXIT_FAILURE);
    }
  
  inet_ntop(p->ai_family, get_in_addr((struct sockaddr *)p->ai_addr),
            s, sizeof s);
  if ((spo->flags & SPO_QUIET) != SPO_QUIET)
    printf("connecting to %s\n", s);
  
  freeaddrinfo(servinfo);
  return sockfd;
}




static size_t
calc_time (struct timespec start, struct timespec stop)
{
  size_t ret;
  if (start.tv_sec != stop.tv_sec)
    {
      ret = (1000000000 - start.tv_nsec) + stop.tv_nsec;
    }
  else
    {
      ret = stop.tv_nsec - start.tv_nsec;
    }

  return ret;
}

static void
_ops_timer_intr (int sig, siginfo_t * extra, void *cruft)
{
  int noverflow;
  server_ping_opts_t *spo = extra->si_value.sival_ptr;
  int errorno;


  if ((noverflow = timer_getoverrun (*(spo->wake_ping_timer))))
    {
      late++;
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr, "okc_server_ping: warning: timer overflow by %d\n", 
		 noverflow);
    }
  else
    {
      if ((errorno = pthread_mutex_trylock (&spo->wait_on_next_ping_mx)) != 0)
	{
	  if (errorno == EBUSY)
	    {
	      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
		fprintf (stderr, "okc-ping-server: warning: ping task not ready,"
			 " skipping (trylock)\n");
	      late++;
	    }
	  else
	    {
	      perror ("okc-ping-server: pthread_mutex_trylock");
	      exit (EXIT_FAILURE); 
	    }
	}
      else
	{
	  if ((spo->flags & OPS_SIGNAL_PLANNER_READY) == OPS_SIGNAL_PLANNER_READY)
	    {
	      if (0 != 
		  pthread_cond_signal (&spo->wait_on_next_ping_cd))
		{
		  perror ("okc-ping-server: _ops_timer_intr: "
			  "pthread_cond_signal");
		  exit (EXIT_FAILURE);
		}
	      if (0 != 
		  pthread_mutex_unlock (&spo->wait_on_next_ping_mx ))
		{
		  perror ("okc-ping-server: _ops_timer_intr: "
			  "pthread_mutex_unlock");
		  exit (EXIT_FAILURE);
		}
	    }
	  else
	    {
	      late++;
	      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
		
		fprintf (stderr, "okc-ping-server: warning: ping task not "
			 "ready, skipping (flag)\n");
	      if (0 != 
		  pthread_mutex_unlock (&spo->wait_on_next_ping_mx ))
		{
		  perror ("okc-ping-server: _ops_timer_intr: "
			  "pthread_mutex_unlock");
		  exit (EXIT_FAILURE);
		}
	    }
	}
    }
  return;
}

static float
time_passed_in_secs (struct timeval start, struct timeval stop)
{
  float ms=0.0;


  ms = (float) (stop.tv_sec - start.tv_sec);  

  if (stop.tv_usec > start.tv_usec)
    ms += ((float) stop.tv_usec - start.tv_usec) / 1000000.0;
  else
    ms += ((float)(1000000 - start.tv_usec) + (stop.tv_usec))/1000000.0 - 1.0;
  return ms;
}

  

static void*
ping_server (void* spo_ptr)
{    
  server_ping_opts_t* spo = spo_ptr;
  struct timeval wait;
  struct timeval start_all;
  struct timeval stop_all;
  struct timespec start_time,stop_time;
  struct timeval start_gtod, stop_gtod,last_gtod;
  int i;
  ssize_t nbytes;
  fd_set readfds;
  int ipoc,ipoc_start;
  size_t good, bad;
  long long int nsecs_total;
  size_t min_nsecs, max_nsecs;
  size_t nanosecs_passed;
  float secs_total = 0.0;
  float secs_passed;
  float min_secs,max_secs;
  sigset_t allsigs;
  double joints[7] = {78.8004,127.1174,-61.2806,86.4107,105.2399,-144.9521, 0.0};
  double position[6] = {33.1614,-603.7832,567.8233,92.1401,-44.1952,92.3466};
  tFriMsrData robotdata;
  tFriCmdData commanddata;
  struct sockaddr_in target_addr;
  struct addrinfo hints, *ai;
  int rv;
  int readsel;
  char portnumber[16];

  sigfillset (&allsigs);
  pthread_sigmask (SIG_BLOCK, &allsigs, NULL);

  gettimeofday (&start_all,NULL);
  gettimeofday (&stop_gtod,NULL);
  max_nsecs = nsecs_total = good = bad = 0;
  min_nsecs = spo->delay_in_nanoseconds;
  max_secs = 0.0;
  min_secs = 1.0;

  ipoc_start = ipoc = 0;
  last_gtod = start_all;

  memset(&hints, 0, sizeof (hints));
  hints.ai_family = AF_INET;
  hints.ai_socktype = SOCK_DGRAM;
  

  snprintf (portnumber,16,"%u",spo->port);

  if ((rv = getaddrinfo(spo->hostname, portnumber, &hints, &ai)) != 0) 
    {
      fprintf(stderr, "ping_server: %s\n", 
	      gai_strerror(rv));
      exit (EXIT_FAILURE);
    }

  target_addr.sin_family = AF_INET;
  target_addr.sin_port = ((struct sockaddr_in*) ai->ai_addr)->sin_port;
  target_addr.sin_addr = ((struct sockaddr_in*) ai->ai_addr)->sin_addr;

  freeaddrinfo (ai);


  for (i=0; i < 7; i++)
    {
      robotdata.data.msrJntPos[i] = M_PI*joints[i]/180.0;
      robotdata.data.cmdJntPos[i] = M_PI*joints[i]/180.0;
    }

  robotdata.intf.state = FRI_STATE_MON;
  while (keep_going)
    {
      
      for (i=0; i<7; i++)
	{
	  robotdata.data.msrJntPos[i] = commanddata.cmd.jntPos[i];
	  /* robotdata.data.msrCartPos[i] = position[i]; */
	  robotdata.data.cmdJntPos[i] = commanddata.cmd.jntPos[i];
	  /* robotdata.data.cmdCartPos[i] = position[i]; */
	}
      robotdata.head.sendSeqCount = ++ipoc;
      robotdata.head.packetSize = FRI_MSR_DATA_SIZE;

      robotdata.robot.power = 1;
      robotdata.intf.quality = FRI_QUALITY_OK;
      robotdata.intf.desiredMsrSampleTime = 0.002;
      robotdata.intf.desiredCmdSampleTime = 0.002;

      if (commanddata.krl.intData[15] != 0)
	fprintf (stderr,"DEBUG: intData[15] = %d\n",commanddata.krl.intData[15]);

      switch (commanddata.krl.intData[15])
	{
	case 1: robotdata.krl.intData[15] = 100;
	  robotdata.krl.intData[14] = 1;
	  robotdata.intf.state = FRI_STATE_CMD;
	  robotdata.krl.intData[13]++;
	  break;
	case 2: robotdata.krl.intData[15] = 101;
	  robotdata.krl.intData[14] = 2;
	  robotdata.intf.state = FRI_STATE_MON;
	  robotdata.krl.intData[13]++;
	  break;
	case 3: robotdata.krl.intData[15] = 0;
	  robotdata.krl.intData[14] = 0;
	  robotdata.krl.intData[13]++;
	  break;
	case 4: robotdata.krl.intData[15] = 102;
	  robotdata.krl.intData[14] = 4;
	  robotdata.krl.intData[13]++;
	  break;
	case 5: robotdata.krl.intData[15] = 103;
	  robotdata.krl.intData[14] = 5;
	  robotdata.krl.intData[13]++;
	  break;
	case 6: robotdata.krl.intData[15] = 104;
	  robotdata.krl.intData[14] = 6;
	  robotdata.krl.intData[13]++;
	  break;
	case 7: robotdata.krl.intData[15] = 105;
	  robotdata.krl.intData[14] = 7;
	  robotdata.krl.intData[13]++;
	  break;
	case 8: robotdata.krl.intData[15] = 106;
	  robotdata.krl.intData[14] = 8;
	  robotdata.krl.intData[13]++;
	  break;
	case 9: robotdata.krl.intData[15] = 107;
	  robotdata.krl.intData[14] = 9;
	  robotdata.krl.intData[13]++;
	  break;
	case 11: robotdata.krl.intData[15] = 111;
	  robotdata.krl.intData[14] = 11;
	  robotdata.krl.intData[13] = 0;
	  break;
	default: robotdata.krl.intData[15] = 0;
	  robotdata.krl.intData[14] = 0;

	}

      if (0 != pthread_mutex_lock (&spo->wait_on_next_ping_mx))
	{
	  perror ("okc-ping-server: ping_server: pthread_mutex_lock");
	  exit (EXIT_FAILURE);
	}
      /* Signal Interrupt Handler that we are ready if he is... */
      spo->flags |= OPS_SIGNAL_PLANNER_READY;

      /* And wait for Interrupt Handler to signal us. */
      if (0 != pthread_cond_wait (&spo->wait_on_next_ping_cd,
				  &spo->wait_on_next_ping_mx)) 
	{
	  perror ("okc-ping-server: ping_server: pthread_cond_wait");
	  exit (EXIT_FAILURE);
	}
      spo->flags ^= OPS_SIGNAL_PLANNER_READY;
      /* Release lock */
      if (0 != pthread_mutex_unlock (&spo->wait_on_next_ping_mx))
	{
	  perror ("okc-ping-server: ping_server: pthread_mutex_unlock");
	  exit (EXIT_FAILURE);
	}
      gettimeofday (&start_gtod,NULL);
      clock_gettime (CLOCK_REALTIME,&start_time);

      nbytes = sendto (spo->sockfd,&robotdata,sizeof (robotdata),0, 
		       (struct sockaddr *) &target_addr, 
		       sizeof (struct sockaddr_in));
      if (nbytes < 0)
	{
	  perror ("okc-ping-server: error: could not send");
	  keep_going = 0;
	}
      else
	{
	  if (FRI_MSR_DATA_SIZE != nbytes)
	    fprintf (stderr,"okc-ping-server: warning: "
		     "incomplete send\n");
	}
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr,"%ld bytes send to %s:%u after %f seconds passed (gtod) ",
		 nbytes,spo->hostname, spo->port,
		 time_passed_in_secs (last_gtod,start_gtod));
      last_gtod = start_gtod;
      FD_ZERO (&readfds);
      FD_SET (spo->sockfd,&readfds);
      wait.tv_sec = 0;
      wait.tv_usec = (spo->delay_in_nanoseconds / 1000) /* - 1000 */;
      if (-1 == (readsel = select (spo->sockfd+1,&readfds,NULL,NULL,&wait)))
	{
	  perror ("select");
	  exit (EXIT_FAILURE);
	}
      if (0 != readsel)
	{
	  /* fprintf (stderr,"Select returned %d, recvfrom()ing now...\n",readsel); */

	  nbytes = recvfrom (spo->sockfd,&commanddata, sizeof (commanddata),
			     0, NULL, NULL);

	  if (commanddata.head.reflSeqCount != ipoc)
	    {
	      FD_ZERO (&readfds);
	      FD_SET (spo->sockfd,&readfds);

	      if (-1 == (readsel = select (spo->sockfd+1,&readfds,NULL,NULL,&wait)))
		{
		  perror ("select");
		  exit (EXIT_FAILURE);
		}

	      if (0 != readsel)
		{
		  bad++;
		  nbytes = recvfrom (spo->sockfd,&commanddata, sizeof (commanddata),
				     0, NULL, NULL);
		}
	    }

	  if (nbytes != FRI_CMD_DATA_SIZE)
	    fprintf (stderr,"okc-ping-server: warning: "
		     "incomplete recieve\n");
      
	  clock_gettime (CLOCK_REALTIME,&stop_time);
	  gettimeofday (&stop_gtod,NULL);
      
	  nanosecs_passed = calc_time (start_time,stop_time);
	  secs_passed = time_passed_in_secs (start_gtod,stop_gtod);
	  if (nanosecs_passed > max_nsecs)
	    max_nsecs = nanosecs_passed;
	  if (nanosecs_passed < min_nsecs)
	    min_nsecs = nanosecs_passed;
	  
	  if (secs_passed > max_secs)
	    max_secs = secs_passed;
	  if (secs_passed < min_secs)
	    min_secs = secs_passed;
	  
	}
      else
	{
	  nbytes = 0;
	}
      
      if (nbytes == 0)
	{
	  if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	    fprintf (stderr," - timeout\n");
	}
      else
	{
	  for (i = 0; i < 7; i++)
	    {
	      joints[i] = commanddata.cmd.jntPos[i];
	    }
	  for (i = 0; i < 7; i++)
	    {
	      position[i] = commanddata.cmd.cartPos[i];
	    }
	  if ((commanddata.head.reflSeqCount) == ipoc && (late == 0))
	    {
	      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
		fprintf (stderr," - received answer after %lu ns (RT) or %f secs (GTOD), ipoc correct\n",
			 nanosecs_passed, time_passed_in_secs (start_gtod,stop_gtod));
	      nsecs_total += nanosecs_passed;
	      secs_total += secs_passed;
	      good++;
	    }
	  else
	    {
	      if (late)
		bad += late;
	      else
		bad++;
	      late = 0;
	      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
		fprintf (stderr,
			 " - received answer after %lu ns (RT) or %f secs (GTOD), ipoc NOT correct (%d vs %d)\n",
			 nanosecs_passed,time_passed_in_secs (start_gtod,stop_gtod),commanddata.head.reflSeqCount,ipoc);
	    }
	  
	}
     
    }
  gettimeofday (&stop_all, NULL);

  clock_getres (CLOCK_REALTIME,&start_time);

  printf ("\nStatistics: Packets Send Total %d\n"
	  "            Packets Received in time %lu (%f %%)\n"
	  "            Late/Lost Packets %lu (%f %%)\n"
	  "            Average time to answer (CLOCK_REALTIME) %f ms (max: %f ms, min %f ms)\n"
	  "            Average time to answer (gettimeofday) %f secs (max: %f secs, min %f secs)\n"
	  "            Ping server had run %f seconds, which results in %f frames per second\n"
	  "            CLOCK_REALTIME says its Timer Resolution is %lu nanosecs\n",
	  ipoc - ipoc_start, good, (float)good*100.0 / (ipoc - ipoc_start),
	  bad, (float) bad*100.0 / (ipoc - ipoc_start),
	  ((float) nsecs_total / (float)good) / 1000000.0,
	  (float) max_nsecs / 1000000.0, 
	  (float) min_nsecs / 1000000.0,
	  secs_total / (float) good,
	  max_secs,
	  min_secs,
	  time_passed_in_secs (start_all, stop_all),
	  (float) (ipoc - ipoc_start) / time_passed_in_secs (start_all, stop_all),
	  start_time.tv_nsec);
  return NULL;
}

void
start_ping_thread (server_ping_opts_t* spo)
{
  struct timespec tp;
  struct sigaction sa;
  struct itimerspec i;
  struct sigevent timer_event;
  pthread_attr_t thread_attr;
  struct sched_param param;
  int result, policy;
  

  sa.sa_flags = SA_SIGINFO;	/*real-time signal */
  sa.sa_sigaction = _ops_timer_intr;	/*pointer to action */

  if (sigaction (SIGRTMIN, &sa, NULL) < 0)
    {
      perror ("okc-ping-server: start_ping_thread: sigaction error");
      exit (EXIT_FAILURE);
    }

  /* catch CTRL-C and such */
  signal (SIGINT, catch_interrupt);
  signal (SIGTERM, catch_interrupt);


  /*
   * first determine whether the desired clock exists
   */

  if (clock_getres (CLOCK_REALTIME, &tp) != 0)
    {
      perror ("okc-ping-server: start_ping_thread: clock_getres");
      exit (EXIT_FAILURE);
    }

  if (2 * tp.tv_nsec > spo->delay_in_nanoseconds)
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	{
	  fprintf (stderr,
		   "okc-ping-server: start_ping_thread:: warning: clock "
		   "resolution of system probably "
		   "\n\t\tinsufficient.\n");
	  fprintf (stderr, "\t\tclock resolution:\t %ld sec, %ld nanosec.\n",
		   tp.tv_sec, tp.tv_nsec);
	  fprintf (stderr, "\t\tdesired intervall:\t %ld sec, %ld nanosec.\n",
		   (long int) 0, spo->delay_in_nanoseconds);
	  
	  fprintf (stderr, "\t\ttrying to continue anyway... good luck!\n");
	}
    }

  /*
   * create a timer based upon the CLOCK_REALTIME clock 
   */

  /* set resolution to the desired values */

  i.it_interval.tv_nsec = spo->delay_in_nanoseconds;
  i.it_interval.tv_sec = 0;
  i.it_value = i.it_interval;

  /*
   * this describes the asynchronous notification to be posted
   * upon this timer's expiration:
   *
   * - use signals
   * - send SIGRTMIN
   * - send extra data consisting of a pointer back to the timer ID
   *   cannot pass the timer ID itself because we haven't created it
   *   yet.
   */

  timer_event.sigev_notify = SIGEV_SIGNAL;
  timer_event.sigev_signo = SIGRTMIN;
  timer_event.sigev_value.sival_ptr = (void *) spo;

  if (timer_create (CLOCK_REALTIME, &timer_event, spo->wake_ping_timer) <
      0)
    {
      perror ("okc-ping-server: start_ping_thread: timer_create");
      exit (EXIT_FAILURE);
    }

  /* relative timer, go off at the end of the interval */

  if (timer_settime (*(spo->wake_ping_timer), 0, &i, NULL) < 0)
    {
      perror ("okc-ping-server: start_ping_thread: timer_settime");
      exit (EXIT_FAILURE);
    }
  
  if (0 != pthread_attr_init (&thread_attr))
    {
      perror ("okc-ping-server: pthread_attr_init");
      exit (EXIT_FAILURE);
    }

  if (0 != pthread_attr_setschedpolicy (&thread_attr, SCHED_FIFO))
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr,"okc-ping-server: warning: no SCHED_FIFO available\n");
    }
  
  

  param.sched_priority = 50;
  if (0 != pthread_attr_setschedparam(&thread_attr, &param))
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr,"okc-ping-server: warning: no SCHED_PRIORITY "
		 "available\n");
    }

  if (0 != pthread_create (&spo->ping_thread, &thread_attr, 
			   ping_server, (void*) spo))
    {
      perror ("okc-ping-server: pthread_create");
      exit (EXIT_FAILURE);
    }

  if (0 != (result = pthread_getschedparam (spo->ping_thread,&policy,
					    &param)))
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	{
	  perror ("okc-ping-server: pthread_getschedparam");
	  fprintf (stderr,"result was %d\n",result);
	}
    }
  else
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr,"okc-ping-server: info: thread is in class "
		 "%s at priority %d\n",
		 (policy==SCHED_OTHER)?"SCHED_OTHER":(policy==SCHED_FIFO)?
		 "SCHED_FIFO":"SCHED_RR", param.sched_priority);
    }

  if (policy != SCHED_FIFO)
    {
      if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	fprintf (stderr,"okc-ping-server: info: altering thread priority\n");
      policy = SCHED_FIFO;
      param.sched_priority = 50;
      if (0 != pthread_setschedparam (spo->ping_thread,policy,&param))
	{
	  if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	    {
	      perror ("okc-ping-server: pthread_setschedparam");
	      fprintf (stderr,"okc-ping-server: warning: failed to set up "
		       "realtime task\n");
	    }
	}
      
      if (0 != (result = pthread_getschedparam (spo->ping_thread,&policy,
						&param)))
	{
	  if ((spo->flags & SPO_QUIET) != SPO_QUIET)
	    {
	      perror ("okc-ping-server: pthread_getschedparam");
	      fprintf (stderr,"result was %d\n",result);
	    }
	}
      else
	{
	  if ((spo->flags & SPO_QUIET) != SPO_QUIET)

	    fprintf (stderr,"okc-ping-server: info: thread is in class %s "
		     "at priority %d\n",
		     (policy==SCHED_OTHER)?"SCHED_OTHER":(policy==SCHED_FIFO)?
		     "SCHED_FIFO":"SCHED_RR", param.sched_priority);
	}
    }

  pthread_attr_destroy (&thread_attr);
}

static void
pimp_me_to_rt (void)
{
  pid_t own_pid;
  struct sched_param param;
  int error;

  own_pid = getpid ();



  param.sched_priority = 55;
  
  if (0 != (error = sched_setscheduler (own_pid,SCHED_FIFO,&param)))
    {
      fprintf (stderr,"failed to alter scheduling parameters\n");
      perror ("sched_setparam");
    }
}
	
  

int 
main(int argc, char **argv)
{
  server_ping_opts_t* spo;

  pimp_me_to_rt();

  spo = parse_args (argc,argv);
  
  spo->sockfd = connect_to_server (spo);
  
  start_ping_thread (spo);
  
  pthread_join (spo->ping_thread,NULL);

  //atexit (print_stats);
  close(spo->sockfd);

  free (spo->hostname);
  free (spo);

  return 0;
}