#include <stdio.h>
#include <math.h>
#include <pthread.h>
#include <sys/neutrino.h>
#include <sys/siginfo.h>
#include <sys/time.h>

#define USE_PULSE          // comment out if using signals

#define BUFFER_SIZE   5000 // buffer size
#define PERIOD    10000000 // in nanoseconds (10ms)
#define PRIORITY     60    // thread priority

void* thread(void* argv){
   
   long long latencies[BUFFER_SIZE];    // hold the latencies
   timer_t timerid;                     // timer ID
   struct itimerspec timer;             // the timer data struct
   struct timespec t1, t2;              // time values
   struct sigevent event;               // event to deliver
   int chid, coid;                      // channel/connection id
   size_t i;

#ifdef USE_PULSE
  
   fprintf(stderr, "Using pulses\n" );
   
   // create the communication channel
   chid = ChannelCreate( _NTO_CHF_FIXED_PRIORITY );
   if( chid == -1 )
     { fprintf( stderr, "Channel not created\n"); }

   // create a connection
   coid = ConnectAttach( 0, 0, chid, 0, 0 );
   if( coid == -1 )
     { fprintf( stderr, "Connection not created\n"); }

   // configure the event pulse
   SIGEV_PULSE_INIT( &event, coid, PRIORITY, 1, 0 );

   // create the timer for the event pulse
   if( timer_create( CLOCK_REALTIME, &event, &timerid ) == -1 )
     { fprintf( stderr, "Timer not created\n"); }

#else
   
   fprintf(stderr, "Using signals\n" );

   // create the timer for SIGALRM 
   if( timer_create( CLOCK_REALTIME, NULL, &timerid ) == -1 )
     { fprintf( stderr, "Timer not created\n"); }
   
#endif
     
   // set timer
   timer.it_value.tv_sec = 0;
   timer.it_value.tv_nsec = PERIOD;
   timer.it_interval.tv_sec = 0;
   timer.it_interval.tv_nsec = PERIOD;

   // start the timer
   timer_settime( timerid, 0, &timer, NULL );

   // time variables
   clock_gettime( CLOCK_REALTIME, &t1 );
   
   // store the latencies
   for( i=0; i<BUFFER_SIZE; i++ ){
      
#ifdef USE_PULSE

      // wait for the pulse
      struct _pulse pulse;
      MsgReceive( chid, &pulse, sizeof(pulse), NULL );
      
#else

      sigset_t waitset;
      int sig;
      sigemptyset( &waitset );
      sigaddset( &waitset, SIGALRM ); 
      // wait for the signal
      sigwait( &waitset, &sig );
      
#endif
      
      // get time
      clock_gettime( CLOCK_REALTIME, &t2 );
      
      // time difference
      latencies[i] = ( ( t2.tv_sec  - t1.tv_sec  )*1000000000 +
		       ( t2.tv_nsec - t1.tv_nsec ) );

      // latency = PERIOD - time difference
      latencies[i] = fabs( PERIOD - latencies[i] );

      t1 = t2;
   }

   for( i=1; i<BUFFER_SIZE; i++ )
     { printf( "%Ld\n", latencies[i]); }

   struct sched_param param;
   sched_getparam( 0, &param );
   fprintf( stderr, "Priority: %d\n", param.sched_priority );

   return NULL;
}

int main(int argc, char **argv){

   pthread_attr_t attr;
   pthread_attr_init(&attr);
   
   // set the priority of the thread explicitly
   pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED );
   // set the policy
   pthread_attr_setschedpolicy(&attr, SCHED_FIFO);

   // set the priority
   struct sched_param param;
   param.sched_priority = PRIORITY;
   pthread_attr_setschedparam(&attr, &param);
   
   pthread_t tid1;
   pthread_create( &tid1, &attr, thread, NULL );
   pthread_join( tid1, NULL );

   return 0;
}