#define _GNU_SOURCE
#include <pthread.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#define LOOPCOUNT 15

void* tH(void*);
void* tL(void*);

void set_cpu_affinity(int cpu);

pthread_mutex_t mutex_dbg;  // buffer mutex (debug output)
pthread_mutex_t mutex_cs;   // critical section mutex

char buffer[10000] = "";

int main(int argc, char* argv[]) {
  pthread_mutexattr_t mutex_attr;

  pthread_t thread_H, thread_L;
  pthread_attr_t attr_H, attr_L;
  struct sched_param param_H, param_L;

  int policy_father;
  struct sched_param param_father;

  set_cpu_affinity(0);  // bind to CPU core 0

  // Father sched
  pthread_getschedparam(pthread_self(), &policy_father, &param_father);
  param_father.sched_priority = sched_get_priority_min(SCHED_RR);
  pthread_setschedparam(pthread_self(), SCHED_RR, &param_father);

  if (pthread_mutexattr_init(&mutex_attr) == 0) {
    pthread_mutexattr_setprotocol(&mutex_attr, PTHREAD_PRIO_INHERIT);  // Enable priority inheritance

    if (pthread_mutex_init(&mutex_dbg, &mutex_attr) == 0) {
      if (pthread_mutex_init(&mutex_cs, &mutex_attr) == 0) {
        if (pthread_attr_init(&attr_H) == 0) {
          if (pthread_attr_init(&attr_L) == 0) {
            // T_H sched
            pthread_attr_setschedpolicy(&attr_H, SCHED_RR);
            param_H.sched_priority = sched_get_priority_max(SCHED_RR);  // Higher priority
            pthread_attr_setschedparam(&attr_H, &param_H);
            pthread_attr_setinheritsched(&attr_H, PTHREAD_EXPLICIT_SCHED);

            // T_L sched
            pthread_attr_setschedpolicy(&attr_L, SCHED_RR);
            param_L.sched_priority = sched_get_priority_min(SCHED_RR);  // Lower priority
            pthread_attr_setschedparam(&attr_L, &param_L);
            pthread_attr_setinheritsched(&attr_L, PTHREAD_EXPLICIT_SCHED);

            if (pthread_create(&thread_L, &attr_L, tL, NULL) == 0) {
              nanosleep((const struct timespec[]){{0, 10000000L}}, NULL);

              if (pthread_create(&thread_H, &attr_H, tH, NULL) == 0) {
                pthread_join(thread_H, NULL);
              }
              pthread_join(thread_L, NULL);
            }
          }
        }
        pthread_mutex_destroy(&mutex_dbg);
        pthread_mutex_destroy(&mutex_cs);
      }
    }
  }
  printf("%s", buffer);
  return 0;
}

void* tH(void* ptr) {
  set_cpu_affinity(0);  // bind to CPU core 0

  pthread_mutex_lock(&mutex_dbg);
  strcat(buffer, "Thread T_H. Attempting to lock mutex_cs!\n");
  pthread_mutex_unlock(&mutex_dbg);

  pthread_mutex_lock(&mutex_cs);
  pthread_mutex_lock(&mutex_dbg);
  strcat(buffer, "Thread T_H has locked mutex_cs!\n");
  pthread_mutex_unlock(&mutex_dbg);

  for (int i = 0; i < LOOPCOUNT; i++) {
    for (int j = 0; j < 1000000; j++) {
      ;  // busy wait
    }
    pthread_mutex_lock(&mutex_dbg);
    strcat(buffer, "I am Thread T_H!\n");
    pthread_mutex_unlock(&mutex_dbg);
  }
  pthread_mutex_unlock(&mutex_cs);

  return NULL;
}

void* tL(void* ptr) {
  set_cpu_affinity(0);  // bind to CPU core 0

  pthread_mutex_lock(&mutex_dbg);
  strcat(buffer, "Thread T_L. Attempting to lock mutex_cs!\n");
  pthread_mutex_unlock(&mutex_dbg);

  pthread_mutex_lock(&mutex_cs);
  pthread_mutex_lock(&mutex_dbg);
  strcat(buffer, "Thread T_L has locked mutex_cs!\n");
  pthread_mutex_unlock(&mutex_dbg);

  for (int i = 0; i < LOOPCOUNT; i++) {
    for (int j = 0; j < 1000000; j++) {
      ;  // busy wait
    }
    pthread_mutex_lock(&mutex_dbg);
    strcat(buffer, "I am Thread T_L!\n");
    pthread_mutex_unlock(&mutex_dbg);
  }
  pthread_mutex_unlock(&mutex_cs);

  return NULL;
}

void set_cpu_affinity(int cpu) {
  cpu_set_t cpuset;
  CPU_ZERO(&cpuset);
  CPU_SET(cpu, &cpuset);
  sched_setaffinity(0, sizeof(cpuset), &cpuset);
}