#ifdef UNICORE
/* _GNU_SOURCE must be defined to use sched_setaffinity() */
#define _GNU_SOURCE
#endif

#include <stdlib.h>
#include <pthread.h>
#include <stdio.h>
#include <sched.h>
#include <string.h>

struct thread_data
{
  int *shared_data;
  pthread_mutex_t *mutex;
  pthread_cond_t *condvar;
  pthread_cond_t *done;
  int *ready_count;
  int value_to_write;
  pthread_t tid;
};

static void *job_body(void *arg)
{
  struct thread_data *thread_data = arg;

  pthread_mutex_lock(thread_data->mutex);
  ++*thread_data->ready_count;
  pthread_cond_wait(thread_data->condvar, thread_data->mutex);
  *thread_data->shared_data = thread_data->value_to_write;
  pthread_cond_signal(thread_data->done);
  pthread_mutex_unlock(thread_data->mutex);

  return NULL;
}

int main(int argc, char *argv[])
{
  int shared_data, old_shared_data, ready_count;
  pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
  pthread_cond_t condvar = PTHREAD_COND_INITIALIZER;
  pthread_cond_t done = PTHREAD_COND_INITIALIZER;
  struct thread_data *thread_data;
  int thread_data_size;
  struct sched_param schedprm = {0};
  pthread_attr_t thread_attr;
#ifdef UNICORE
  cpu_set_t cores;
  CPU_ZERO(&cores);
  CPU_SET(0, &cores);
  sched_setaffinity(0, sizeof(cores), &cores);
#endif

  /* Create the data of every non-main thread */
  thread_data_size = (sched_get_priority_max(SCHED_FIFO)
                      - sched_get_priority_min(SCHED_FIFO) + 1);
  thread_data = malloc(sizeof(*thread_data) * thread_data_size);
  if (!thread_data) {
    fprintf(stderr, "Error: Cannot allocate memory to hold thread data\n");
    return -1;
  }
  for (int i = 0; i < thread_data_size; ++i) {
    thread_data[i].shared_data = &shared_data;
    thread_data[i].mutex = &mutex;
    thread_data[i].condvar = &condvar;
    thread_data[i].done = &done;
    thread_data[i].ready_count = &ready_count;
    thread_data[i].value_to_write = 1 + i;
  }
  fprintf(stderr, "As many as %d threads will be created\n", thread_data_size);

  /* Prepare the attributes shared by every non-main thread */
  pthread_attr_init(&thread_attr);
  pthread_attr_setinheritsched(&thread_attr, PTHREAD_EXPLICIT_SCHED);
  pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);

  for (int k = 0; k < 1000; ++k) {
    ready_count = 0;
    
    /* Create threads such that a thread_i has priority
       sched_get_priority_max(SCHED_FIFO) - i */
    schedprm.sched_priority = sched_get_priority_max(SCHED_FIFO);
    for (int i = 0; i < thread_data_size; ++i) {
      schedprm.sched_priority -= i;
      pthread_attr_setschedparam(&thread_attr, &schedprm);
      int rc;
      if (rc = pthread_create(&thread_data[i].tid, &thread_attr,
                              job_body, thread_data + i)) {
        fprintf(stderr, "Error: Cannot create thread #%d: %s",
                i + 1, strerror(rc));
        free(thread_data);
        return -1;
      }
    }

    /* Wait until every non-main thread has waited on the condvar */
    pthread_mutex_lock(&mutex);
    while (ready_count != thread_data_size) {
      pthread_mutex_unlock(&mutex);
      pthread_mutex_lock(&mutex);
    }

    /* Release just one thread from the condvar's queue */
    pthread_cond_signal(&condvar);

    /* Let's see now which thread gets dequeued */
    pthread_cond_wait(&done, &mutex);

    /* If the next owner of the condition variable's mutex is determined by the
       scheduling policy and parameter, the value of shared_data will never
       change.  And, the value is none other than one because the thread with
       the highest SCHED_FIFO priority will run first. */
    if (k) {
      if (shared_data != old_shared_data) {
        old_shared_data = shared_data;
        fprintf(stderr, "shared_data is now %d (k = %d)\n", shared_data, k);
      }
    } else {
      fprintf(stderr, "shared_data is %d (k = %d)\n", shared_data, k);
      old_shared_data = shared_data;
    }

    /* Let every other non-main thread terminate */
    pthread_cond_broadcast(&condvar);
    pthread_mutex_unlock(&mutex);
    for (int i = 0; i < thread_data_size; ++i) {
      pthread_join(thread_data[i].tid, NULL);
    }
    
  }

  free(thread_data);
  return 0;

}