#include <math.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/io.h>
#include <sys/mman.h>

#include <native/task.h>
#include <native/queue.h>
#include <native/intr.h>


#define STACK_SIZE 8192
#define STD_PRIO1 2
#define STD_PRIO2 1
#define QUEUE_INPUT_LEN		1024
#define USE_READ_WRITE
RT_TASK zaehler1_task_ptr;
RT_TASK zaehler2_task_ptr;
int count1 = 0;
int count2 = 0;
int i;
int end = 0;
typedef struct  {
	int counter;
	int data;
}TInputData;

int missedpackets=0;

RT_QUEUE queue_input;


//                      --s-ms-us-ns
RTIME task_period_ns1 =   10000000llu;
RTIME task_period_ns2 =  10000000000llu;

void zaehler1_task(void *cookie){
        
  int ret;
  TInputData sendData;
   memset(&sendData, 0, sizeof(TInputData));



// ************************* Xenomai-Krempel ********************************************************************
        ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(task_period_ns1));
        if (ret) {
                printf("error while set periodic, code %d\n",ret);
                return;
        }
// ************************* Ende Xenomai-Krempel ****************************************************************


// ********************** Beginn des wiederholt ausgefuehrten Codes **********************************************
        while(!end){
            
               rt_task_wait_period(NULL);


		void *msg = rt_queue_alloc(&queue_input, sizeof(TInputData));
		if(msg == NULL) {
			printf("rt_queue_alloc(queue_inout, %d) failed\n", sizeof(TInputData));
			
                     }
		memcpy(msg, &sendData, sizeof(TInputData));
		int bytesSent = rt_queue_send(&queue_input, msg,sizeof(TInputData), Q_BROADCAST);
		
                  if (bytesSent <= 0) {
		  	
                    printf("rt_queue_send(queue_input) failed sent bytes: %d\n", bytesSent);
			rt_queue_free(&queue_input, msg);
		 	
		}

		
                sendData.counter++;  
      }
// ********************** Ende des wiederholt ausgefuehrten Codes ***********************************************
}




// signal-handler, to ensure clean exit on Ctrl-C
void clean_exit(int dummy) {
	printf("cleanup\n");
	end = 1;
	rt_task_delete(&zaehler1_task_ptr);
	rt_task_delete(&zaehler2_task_ptr);
	printf("end\n");
}

int main(int argc, char *argv[]) {
	int err, ret;
	
       int res;
	res = mlockall(MCL_CURRENT | MCL_FUTURE);
	if (res < 0) {
		printf("mlockall failed: %d\n", res);
		
	}
        // INPUT_QUEUE
	res = rt_queue_create(&queue_input, "queue_input", sizeof(TInputData) * QUEUE_INPUT_LEN,
		QUEUE_INPUT_LEN, Q_FIFO | Q_SHARED);
	if (res == -EEXIST)  {
		res = rt_queue_bind(&queue_input, "queue_input", 1000);
		//rt_queue_clear(&queue_input);
	}
	if (res < 0) {
		printf("rt_queue_create(queue_input) failed: %d\n", res);
		
	}

	

 
       printf("start\n");
	// install signal handler
	signal(SIGTERM, clean_exit);	
	signal(SIGINT, clean_exit);	

	/* create zaehler1_task */
	err = rt_task_create(&zaehler1_task_ptr,"alpha",STACK_SIZE,STD_PRIO1,0);

	/* start zaehler1_task */
	err = rt_task_start(&zaehler1_task_ptr,&zaehler1_task,NULL);

	// wait for signal & return of signal handler
	pause();
	fflush(NULL);
	return 0;
}