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

static int count;
static int goon = 1;

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)
{
        struct sched_param  param;
        int policy;

        pthread_getschedparam(pthread_self(), &policy, &param);
        printf("%s: policy=%d prio=%d\n",
               __FUNCTION__, policy, param.sched_priority);

        param.sched_priority = LOW_PRIO;
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

        while (goon) {
#ifdef WITH_PRINTF
		pthread_set_mode_np(0, PTHREAD_PRIMARY);
                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;
        struct sched_param  param = { .sched_priority = HIGH_PRIO };

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

        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

#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 > 500) {
                        loop = 0;
                        step = 2;
                        //goon = 0;
                        pthread_kill(id_low, SIGUSR1);
                        printf("SIGUSER1 to id_low: count=%d, overruns=%lu\n",
                               count, overruns);
                }
                else if (step == 2 && loop > 500) {
			loop = 0;
			step = 1;
			pthread_kill(id_low, SIGUSR2);
                        printf("SIGUSER2 to id_low: count=%d, overruns=%lu\n",
                               count, overruns);
		}
                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

static void suspend_signal_handler(int sig)
{
   sigset_t signal_set;

   printf("suspend signal handler\n");
   sigfillset (&signal_set);
   sigdelset (&signal_set, SIGUSR2);
   sigsuspend(&signal_set);

   return;
}

static void resume_signal_handler(int sig)
{
   printf("resume signal handler\n");
   return;
}


void init_signals(sigset_t *mask)
{
   struct sigaction sigusr1, sigusr2;

   sigemptyset(mask);
   sigaddset(mask, SIGINT);
   sigaddset(mask, SIGTERM);
   sigaddset(mask, SIGHUP);
   sigaddset(mask, SIGALRM);

   pthread_sigmask(SIG_BLOCK, mask, NULL);

   /*
    * Install the signal handlers for suspend/resume.
    */

   sigemptyset (&sigusr1.sa_mask);
   sigusr1.sa_flags = 0;
   sigusr1.sa_handler = suspend_signal_handler;
   sigaction(SIGUSR1, &sigusr1, NULL);

   sigemptyset (&sigusr2.sa_mask);
   sigusr2.sa_flags = 0;
   sigusr2.sa_handler = resume_signal_handler;
   sigaction(SIGUSR2, &sigusr2, NULL);

}

int main(int argc, char *argv[])
{
	sigset_t mask;
	int sig;

        mlockall(MCL_CURRENT|MCL_FUTURE);

#ifdef WITH_SIGXCPU
        signal(SIGXCPU, catch_switch);
#endif
#if 0
        signal(SIGTERM, catch_signal);
        signal(SIGINT, catch_signal);
        signal(SIGHUP, catch_signal);
#else
	init_signals(&mask);
#endif

        init_task(HIGH_PRIO);


#if 0
        pthread_join(id_high, NULL);
#else
	sigwait(&mask, &sig);
#endif

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