#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <signal.h>
#include <netdb.h>
#include "pscomm.h"
#include <sched.h>

#define MIN_TIME_BETWEEN_INTERRUPTS 0

#define MSG_SIZE 1500

unsigned long inaddr = 0;
int port = 0;

static int sock;
static struct sockaddr_in servaddr;
static char buffer[MSG_SIZE+1];
int *pnum;

struct TimeTable tt;
int ttIndex = 0;

int ii = 0;
int diff = 0;
int meanDiff = 0;
int maxDiff = 0;
int minDiff = 0;
int mCount = 0;
int eCount = 0;

// shm pointers;
volatile char * shm1, *shm2, *shmControl;
 
// readShm initialized in init_module()
struct TimeTable *readShm[2]; 

char reqShm[2] = { REQ_2, REQ_1 };
char urgReqShm[2] = { URG_REQ_2, URG_REQ_1 };

static struct timeval cTime;

int firstInterrupt = 1;
int initPeriod = 1;
int waitingForData = 1;
int lateIndicator = 0;
long long alarmCount = 0;
long long prevAlarmCount = 0;
int cleanup = 0;
unsigned long cpu_hz;

// send_routine variables

        struct TimeTable *ttptr;
        long long period;
        long long now, sendTime, startTime, estNextInterrupt, timeToNextInterrupt, timeToNextSend, nextTarget;
        long long prevt, tdiff, cput;
	int i, prev, idiff;
        int ret;
        int packetSize;
        unsigned long tlow0, thigh0, tlow100, thigh100;
// scheduler ..
struct sched_param schedparam;


/* Timing routine */
int send_routine() {

        register unsigned long eax asm("ax");
        register unsigned long edx asm("dx");
/*
        __asm__("rdtsc"
                :"=a" (eax), "=d" (edx));    
*/

   if (cleanup == 1) return;
   alarmCount++;

   // Initialization

   if ( firstInterrupt == 1) {
     // Things to be done only once on the first execution of this routine
     firstInterrupt = 0;

     // order the write of the first table
     shmControl[0] = REQ_1;

     //rtl_printf("FIRST TT Ordered\n");

     i = 0;
     prev = 0;
     prevt = 0;
     estNextInterrupt = 0;
     period = 0;

     // gethrtime()
     gettimeofday(&cTime,(struct timezone*)0);
     now = cTime.tv_sec;
     now *= 1000000000;
     now +=  cTime.tv_usec*1000;

        /* Read the Time Stamp Counter */
        __asm__("rdtsc"
                :"=a" (tlow0), "=d" (thigh0));
                                   
     ualarm(1,0);
     return;
   }

   // Try to estimate the period
   if ( initPeriod == 1) {

      prevt = now;

      // gethrtime()
      gettimeofday(&cTime,(struct timezone*)0);
      now = cTime.tv_sec;
      now *= 1000000000;
      now +=  cTime.tv_usec*1000;
 
      // now = cTime.tv_sec*1000000000 + cTime.tv_usec*1000;

      period += now - prevt;
      
      if (alarmCount == 101) {

         initPeriod = 0;

         period = period/100;

         __asm__("rdtsc"
                  :"=a" (tlow100), "=d" (thigh100));

         
   /* 64-bit subtract - gcc just messes up with long longs */
                __asm__("subl %2,%0\n\t"
                        "sbbl %3,%1"
                        :"=a" (tlow100), "=d" (thigh100)
                        :"g" (tlow0), "g" (thigh0),
                         "0" (tlow100), "1" (thigh100));           
cput = thigh100;
cput = (cput<<32)|tlow100;

fprintf(stderr,"cput:%Ld\n", cput);
                                     
fprintf(stderr,"The estimated basic period is: %Ld [nsec]\n", period);

         estNextInterrupt = now + period;
  
         startTime = now;
      } else {
         ualarm(1,0);
         return;
      }
   }


// Start the infinit sending loop 

   if ( (waitingForData == 1) && (shmControl[0] != ACK) ) {
      // Waiting for data to arrive
 
      //rtl_printf("Waiting for data\n");

      // wait until timing info is read from shm
      gettimeofday(&cTime,(struct timezone*)0);
      now = cTime.tv_sec;
      now *= 1000000000;
      now +=  cTime.tv_usec*1000;
 
      //     now = cTime.tv_sec*1000000000 + cTime.tv_usec*1000;
      estNextInterrupt = now + period;


      // Indicate waiting in the controll message
      if (lateIndicator == 0) {
         lateIndicator = 1;
      }
      ualarm(1,0);
      return;
      
   }

   if ( (waitingForData == 1) && (shmControl[0] == ACK) ) {
      // data received
      waitingForData = 0;

      // set ttptr to the new time table
      ttptr = readShm[ttIndex];

      //rtl_printf("Data already available\n");

      // order the write of the next table
      // and indicate waiting in the controll message
      // if needed
      if (lateIndicator == 1) {
         lateIndicator = 0;
         shmControl[0] = urgReqShm[ttIndex];
      } else {
         shmControl[0] = reqShm[ttIndex];
      }

      // switch the ttIndex to the next table
      ttIndex = (ttIndex+1) % 2;
	
      /* send packets according to the target time table */
      i = 0;
      nextTarget = ttptr->targetTime[0];
      packetSize = ttptr->packetSize[0];
      if (packetSize > MSG_SIZE) packetSize = MSG_SIZE;  
   }


   /*!!! THE LAST ELEMENT IN targetTime MUST BE STOP_SENDING !!!*/
   
   while (nextTarget != STOP_SENDING) {

      sendTime = startTime + nextTarget;

      // gethrtime()
      if (gettimeofday(&cTime,(struct timezone*)0) <0) {
         fprintf(stderr,"gettimeofday error\n");
         return;
      };
      now = cTime.tv_sec;
      now *= 1000000000;
      now +=  cTime.tv_usec*1000;
       //now = cTime.tv_sec*1000000000 + cTime.tv_usec*1000;
      if ( alarmCount != prevAlarmCount) {
         // if this is the first packet sent from this interrupt 
         estNextInterrupt = now + period;
         prevAlarmCount = alarmCount;
      }

      timeToNextSend = sendTime - now;
      timeToNextInterrupt = estNextInterrupt - now;           

      if ( timeToNextSend > MIN_TIME_BETWEEN_INTERRUPTS && 
           timeToNextSend > period ) {
         /* go to sleep */
         //rtl_printf("%i\n",i);

         // might be wiser to try to predict for how long time the process can sleep
         ualarm(1,0);
         return;
      } else {
	  /* Active wait */
          while (sendTime > now) {
             // gethrtime()
             if (gettimeofday(&cTime,(struct timezone*)0) < 0) {
                fprintf(stderr,"gettimeofday error\n");
                return;
             };
             now = cTime.tv_sec;
             now *= 1000000000;
             now +=  cTime.tv_usec*1000;
             //now = cTime.tv_sec*1000000000 + cTime.tv_usec*1000;
          }
          if (nextTarget == NEW_TIME_SERIES) {
             startTime = now;
          }

          /* Send the packet */

          diff = now - sendTime;
	  
          ret=sendto(sock,buffer,packetSize,0,
	      (struct sockaddr *)&servaddr,sizeof(servaddr));
          if (ret<0) {
             fprintf(stderr,"sendto ERROR\n");
          }
          // increase the packet serial number
	  pnum[0]++;

          /* statistics on timing accuracy */
          if (ii == 1) {
	     // == 1, because the 0th diff is an invalid value 
             minDiff = diff;
             maxDiff = diff;
          }
          if (ii >= 1) { 
             if ( nextTarget != NEW_TIME_SERIES ) {
                // Calculate statistics
                // mean
                meanDiff += diff;

                // minimum diff
                if ( diff < minDiff ) { 
                   minDiff = diff;
                }

                // maximum diff
                if ( diff > maxDiff ) {
                   maxDiff = diff;
                }

                // # of diff-s exceeding a limit
                if ( diff > 1000 ) {
                   mCount++;
                }
             }
          }
          // sent counter
          ii++;        

          // sent from this block counter
          i++;        

          nextTarget = ttptr->targetTime[i];
          packetSize = ttptr->packetSize[i];
          if (packetSize > MSG_SIZE) packetSize = MSG_SIZE;
      } 

   }
   //rtl_printf("Finished sending\n");
   waitingForData = 1;

   // end of the infinit sending loop
   ualarm(1,0);
   return;
}



/* main */
main(int argc, char *argv[]) {

   int ret;

   // processing input parameters

   struct hostent *hp;

   if ( argc!= 3 ) {
      fprintf(stderr,"Usage: %s <ipaddr or hostname> <port number>\n", argv[0]);
      return 0;
   }

   if ( (hp = gethostbyname(argv[1])) == NULL ) {
      if ( (inaddr = inet_addr( argv[1] )) == -1) {
         fprintf(stderr,"Error - gethostbyname could not resolve name: %s\n", argv[1]);
         return 0;
      }
   } else {
      inaddr = *((int*)(hp->h_addr_list[0]));
   }
 

   if ( sscanf( argv[2], "%d", &port ) == 0 ) {
      fprintf(stderr,"Error - wrong port number: %s\n", argv[2]);
   }

   if ( ps_init() < 0 ) {
      fprintf(stderr,"Init failed\n");
      return 1;
   }         

   pnum = (int*)buffer;

   ps_getptrs(&shm1, &shm2, &shmControl);   
   shmControl[0] = ACK;

   readShm[0] = (struct TimeTable*)shm1;
   readShm[1] = (struct TimeTable*)shm2; 

   // fprintf(stderr,"shm1 %p\nshm2 %p\nshmC %p\n", shm1, shm2, shmControl);

   // message contents setup - fill with 'A'
   for(ret = 0; ret<MSG_SIZE; ret++) {
      if (ret<1024) buffer[ret] = 65;
      else buffer[ret] = 66;
   }
   buffer[ret] = 0;
   // initial packet serial number
   pnum[0] = 0;

   // socket setup for sending
	
   sock=socket(AF_INET,SOCK_DGRAM,0);
   if (sock < 0) {
      fprintf(stderr,"Error in socket()\n");
      return 1;
   }

   memset(&servaddr,0,sizeof(servaddr));
   servaddr.sin_family = AF_INET;
   servaddr.sin_port = 0;
   servaddr.sin_addr.s_addr = htonl(INADDR_ANY);

   ret=bind(sock,(struct sockaddr *)&servaddr,sizeof(servaddr));
   if (ret < 0) {
      fprintf(stderr,"Error in bind()\n");
      return 1;
   }   

   memset(&servaddr,0,sizeof(servaddr));
   servaddr.sin_family = AF_INET;
   servaddr.sin_port = htons(port);
   servaddr.sin_addr.s_addr = inaddr;

   // Sigalarm configuration    
   signal(SIGALRM, (void(*) (int)) send_routine);

   schedparam.sched_priority = sched_get_priority_max(SCHED_FIFO); 

   fprintf(stdout,"RESP: %d \n", sched_setscheduler(0, SCHED_FIFO, &schedparam));

   send_routine();

   /*WAIT FOR A CHARACTER TO BE ENTERED*/
   fprintf(stdout,"Press Enter to finish!\n");
   ret = getchar();

   /*clean-up*/
   cleanup = 1;

   // print results - statistics on accuracy
   fprintf(stdout,"Statistics: \nmeanDiff :%i\nminDiff  :%i\nmaxDiff  :%i\neCount   :%i\n",meanDiff/(ii-1),minDiff, maxDiff, eCount);

   fprintf(stdout,"ii       :%i\nmCount   :%i\n", ii, mCount);

   // clean-up

   // close sockets
   close(sock);

   // shared memory
   ps_cleanup(); 

   return 0;
}