/* rtps.c

   RT-Linux packet sender module

*/

#include <rtl.h>
#include <time.h>
#include <pthread.h>
#include <rtl_fifo.h>
#include <rtnet/rtnet.h>
#include <mbuff.h>

#include "pscomm.h"

#define MIN_TIME_BETWEEN_INTERRUPTS 0

#define PERIOD 50000
#define MSG_SIZE 1500


// configuration parameters for INSMOD
unsigned int addr, port;

MODULE_PARM(addr, "i");
MODULE_PARM(port, "i");

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

pthread_t thread;

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;


/* the contents of shared memory may change at any time, thus volatile */
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 };


// my clock
#define my_gethrtime gethrtime
/*
hrtime_t my_gethrtime(void) {

   hrtime_t now;
   register unsigned long eax asm("ax");
   register unsigned long edx asm("dx");

   // Read the Time Stamp Counter 
   __asm__("rdtsc"
            :"=a" (eax), "=d" (edx));           

   now = edx;
   now = (now << 32) | eax;

   return now;
}
*/


/* Timing thread routine */
void * send_routine(void *arg) {
        struct TimeTable *ttptr;
        hrtime_t now, sendTime, startTime, estNextInterrupt, timeToNextInterrupt, timeToNextSend, nextTarget;
        hrtime_t prevt, tdiff;

        hrtime_t my_period, start_tsc; 

	int i, prev, idiff;
        int ret;
        int packetSize;

	struct sched_param p;
	p . sched_priority = sched_get_priority_max(SCHED_FIFO);


        /* thread init */
	pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);
	pthread_make_periodic_np (pthread_self(), gethrtime(), PERIOD);


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

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

        /* warm-up */
        i = 0;
        prev = 0;
        prevt = 0;
        estNextInterrupt = 0;
        now = 0;
        start_tsc = my_gethrtime();
        while (i < 16384) {
		pthread_wait_np ();
/*                prevt = now;
                now = gethrtime();
                if ( (now - estNextInterrupt) > (PERIOD - 5000) && i>0 ) {
		   // PROBLEM WITH THE SCHEDULER
                   // A PERIOD WAS LEFT OUT
                   eCount++;
                   idiff = i - prev;
                   prev = i;
                   tdiff = now -prevt;

                   rtl_printf("d %i %Ld \n",i, tdiff);
                   rtl_printf("%i\n",idiff);
                }
                estNextInterrupt = now + PERIOD;
*/
                i++;
        }
        my_period = (my_gethrtime() - start_tsc);
        my_period =  5000 + (my_period >> 14);
rtl_printf("My_period: %Ld\n",my_period);
//        startTime = my_gethrtime();

// Start the infinit sending loop 
 while (1) {

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

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

           // wait until timing info is read from shm        
           while (shmControl[0] != ACK) {
              pthread_wait_np ();
              now = my_gethrtime();
              // estNextInterrupt = now + PERIOD;
              estNextInterrupt = now + my_period;
           }
           // Indicate waiting in the controll message
           // order the write of the next table
           shmControl[0] = urgReqShm[ttIndex];

	   //rtl_printf("Data received\n");
	} else {
	   //rtl_printf("Data already available\n");

           // order the write of the next table
           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;

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

           sendTime = startTime + nextTarget;

           now = my_gethrtime();
           timeToNextSend = sendTime - now;
           timeToNextInterrupt = estNextInterrupt - now;           

//           if ( timeToNextSend > MIN_TIME_BETWEEN_INTERRUPTS && 
//                timeToNextSend > PERIOD ) {
           if ( timeToNextSend > my_period ) {
             /* go to sleep */
             //rtl_printf("%i\n",i);
             pthread_wait_np();
             now = my_gethrtime();
             estNextInterrupt = now + my_period; 
           } else {
	     /* Active wait */
             while (sendTime > now) {
                now = my_gethrtime();
             }
             if (nextTarget == NEW_TIME_SERIES) {
                startTime = my_gethrtime();
             }

             /* Send the packet */

             diff = my_gethrtime() - sendTime;

             ret=rt_sendto(sock,buffer,packetSize,0,
		 (struct sockaddr *)&servaddr,sizeof(servaddr));
             if (ret<0) {
                rtl_printf("sendto ERROR\n");
             }
             /* 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");

 } 
 // end of the infinit sending loop
	return 0;
}



/* Module initialization - INSMOD */

int init_module(void) {
	int ret;

        #ifndef PS_RT_LINUX
           THIS_FILE_MUST_BE_COMPILED_WITH_pscomm_h_FOR_RT_LINUX;
        #endif

        pnum = (int*)buffer;

        // init param test 
	if ( addr == 0 || port == 0 ) {
           rtl_printf("Usage: insmod rtps.o addr=<binary ip address in network byte order> port=<destination port number>\n");
           return -1;
	}

        if ( addr == 0xffffffff ) {
           rtl_printf("Wrong IP address\n");
           return -1;
        }

        // shared memory allocation

        shm1 = (volatile char*) mbuff_alloc("tt1",sizeof(tt));
        shm2 = (volatile char*) mbuff_alloc("tt2",sizeof(tt));   
        if( shm1 == NULL || shm2 == NULL ) {
                rtl_printf("mbuff_alloc failed\n");
        }         

        shmControl = (volatile char*) mbuff_alloc("control",1024);   
        if( shmControl == NULL) {
                rtl_printf("mbuff_alloc failed\n");
        }          
        shmControl[0] = ACK;

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


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

	// socket setup for sending
	
	sock=rt_socket(AF_INET,SOCK_DGRAM,0);
        if (sock < 0) {
           rtl_printf("Error in rt_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=rt_bind(sock,(struct sockaddr *)&servaddr,sizeof(servaddr));
        if (ret < 0) {
           rtl_printf("Error in rt_bind()\n");
           return -1;
        }

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

        // create timing thread
	return pthread_create (&thread, NULL, send_routine, 0);
}




/* Module clean-up - RMMOD */

void cleanup_module(void) {
   //   int ret;
   // int dif1, dif2;

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

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

   // clean-up

   // close sockets
   rt_close(sock);

   // delete threads;
   pthread_delete_np (thread);

   // shared memory
 
   mbuff_free("tt1",(void*)shm1);
   mbuff_free("tt2",(void*)shm2);       
   mbuff_free("control",(void*)shmControl);       

}