#include <stdio.h>
#include <pthread.h>
#include <signal.h>
#include <sys/mman.h>

static int count;

static pthread_t id_low;
static pthread_t id_high;

#define LOW_PRIO 5
#define HIGH_PRIO 10

#undef WITH_PRINTF
#undef WITH_SIGXCPU

void init_task(int prio);

void* low_prio_task(void* dummy)
{

   while (1)
   {
#ifdef WITH_PRINTF
	   printf("%s %d\n", __FUNCTION__, count);
#endif
	   count++;
   }
   printf("Exiting %s, count=%d\n", __FUNCTION__, count);
   return 0;
}

void* high_prio_task(void* dummy)
{
	struct timespec tstart = { .tv_sec = 0, .tv_nsec = 0};
#ifdef WITH_PRINTF
	struct timespec tperiod = { .tv_sec = 0, .tv_nsec = 10000000 };
#else
	struct timespec tperiod = { .tv_sec = 0, .tv_nsec = 1000000 };
#endif
	unsigned long overruns = 0;
	int tick = 0, step = 0, loop = 0, err;

	printf("Starting %s\n", __FUNCTION__);

#ifdef WITH_SIGXCPU
	pthread_set_mode_np(0, PTHREAD_WARNSW);
#endif
	err = pthread_make_periodic_np(pthread_self(), &tstart, &tperiod);
	if (err)
		return (void *)err;

	while (1) {
		err = pthread_wait_np(&overruns);
#if 0
		if (err) {
			pthread_kill(id_low, SIGSTOP);
			printf("%s overruns=%lu, tick=%d, count=%d, err=%d\n",
			       __FUNCTION__, overruns, tick, count, err);
			return 0;
		}
#endif
		if  (step == 0 && loop > 100) {
			loop = 0;
			step = 1;
			init_task(LOW_PRIO);
		}
		else if (step == 1 && loop > 50) {
			loop = 0;
			step = 0;
			pthread_kill(id_low, SIGSTOP);
			printf("Killed low_prio task: count=%d, overruns=%lu\n",
			       count, overruns);
			return 0;
		}
		tick++;
		loop++;
	}
	printf("Exiting %s\n", __FUNCTION__);
	return 0;
}

void init_task(int prio)
{
   struct sched_param parm;
   pthread_attr_t attr;

   pthread_attr_init(&attr);
   if (prio == HIGH_PRIO)
	   pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
   else
	   pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
   pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
   parm.sched_priority = prio;
   pthread_attr_setschedparam(&attr, &parm);

   if (prio == HIGH_PRIO) {
	   pthread_create(&id_high, &attr, high_prio_task, NULL);
	   pthread_set_name_np(id_high,"high_prio");
   } else {
	   pthread_create(&id_low, &attr, low_prio_task, NULL);
	   pthread_set_name_np(id_low,"low_prio");
   }
}

void catch_signal(int sig)
{
	printf("Signal %d catched\n", sig);
}

#ifdef WITH_SIGXCPU
void catch_switch(int sig)
{
	void *bt[32];
	int nentries;

	printf("Signal %d catched\n", sig);

	/* Dump a backtrace of the frame which caused the switch to
	   secondary mode: */
	nentries = backtrace(bt,sizeof(bt) / sizeof(bt[0]));
	backtrace_symbols_fd(bt,nentries,fileno(stdout));
}
#endif

int main(int argc, char *argv[])
{
	mlockall(MCL_CURRENT|MCL_FUTURE);

	signal(SIGTERM, catch_signal);
	signal(SIGINT, catch_signal);
	signal(SIGHUP, catch_signal);
#ifdef WITH_SIGXCPU
	signal(SIGXCPU, catch_switch);
#endif
	init_task(HIGH_PRIO);

	pthread_join(id_high, NULL);

	printf("Exiting %s\n", __FUNCTION__);
	return 0;
}