#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include <unistd.h>
#include <sys/syscall.h>

#ifdef SYS_gettid
#define gettid() syscall(SYS_gettid)
#else
#error "SYS_gettid unavailable on this system"
#endif

#define CHECK(x) if ((x) != 0) { printf("ERROR: %s == %d (file: %s line: %d )\n",#x,x,__FILE__,__LINE__); exit(x); }
#define ASSERT(x) CHECK(!(x))

// Twice this number of threads is started initially:
#define NUM_THREADS (50*10*2)
// 1+this number of threads is kept:
#define NUM_THREADS_NONZERO_TO_KEEP (1)
#define NUM_ITERATIONS_UNDER_LOCK 100000
#define NUM_ITERATIONS_UNTIL_REPORT 100000
#define NUM_SECONDS_TO_RUN_UNTIL_RESTART (10*60)

/* Mutex to be used concurrently */
pthread_mutex_t mutex;
/* statistics */
int num_threads_started = 0;
/* some maps for bookkeeping */
int thread_num_to_tid[NUM_THREADS];
int thread_num_to_prio[NUM_THREADS];

/* function to run in thread */
void *helper(void* dummy)
{
  /* This is just a number for identifying the thread, also used to
   * assign different priority to the threads */
  const long _dummy = (long)dummy;
  /* Get thread ID (available only inside thread), and store in
   * global map. This is used in main() to make sure we have at
   * least one thread with thread ID that has a low byte of 0x00.
   */
  const pid_t tid = gettid();
  thread_num_to_tid[_dummy] = tid;
  /* Atomically count number of threads, not needed, just statistics */
  __atomic_fetch_add(&num_threads_started, 1, __ATOMIC_SEQ_CST);
  /* Some statistics, not modified within critical section. */
  int cnt = 0;
  int __nusers = 0;
  int __nusers2 = 0;
  int __nusers3 = 0;
  /* Only for logging */
  const int prio = thread_num_to_prio[_dummy];
  while (1)
  {
    const int r = random();
    const unsigned int __lock = mutex.__data.__lock;
    __nusers += (mutex.__data.__nusers > 0 ? 1 : 0);
    __nusers2 += (mutex.__data.__owner != 0 ? 1 : 0);
    __nusers3 += (__lock != 0 ? 1 : 0);
    /* Try to acquire mutex, here we see the pthread assertion in
     * case the problem is present.*/
    CHECK(pthread_mutex_lock(&mutex));
    /* do something simple in critical section */
    if (1 == (r ^ 0xf))
    {
      /* do we really have the lock ?
         Mask out Waiters bit*/
      const pid_t tid_ = mutex.__data.__lock & 0x3fffffff;
      ASSERT(tid_ == tid);
    }
    /* unlock mutex */
    CHECK(pthread_mutex_unlock(&mutex));
    /* If the thread is not selected for running it will terminate here */
    if (0 == thread_num_to_tid[_dummy])
    {
      printf("tid=0x%x: T%ld: EXITING...\n", tid, _dummy);
      fflush(stdout);
      break;
    }
    /* periodically show a sign of live by logging, cnt also
     * indicates how often the mutex has been acquired.
     */
    if (0 == (cnt++ % NUM_ITERATIONS_UNTIL_REPORT))
    {
      printf("tid=0x%x: T%ld: prio=%d: %d lock/unlock, __nusers=%d/%d/%d __lock=%s0x%x\n",
             tid, _dummy,
             prio,
             cnt,
             __nusers, __nusers2, __nusers3,
             __lock == 0 ? "!" : "",
             __lock);
      __nusers = 0;
      __nusers2 = 0;
      __nusers3 = 0;
    }
  }
  return NULL;
}

int main(int argc, char *argv[])
{
  pthread_t t1[NUM_THREADS];
  pthread_attr_t attr;
  pthread_mutexattr_t mutexattr;
  if (argc != 1)
  {
    printf("usage: %s\n\n ",argv[0]);
    return -1;
  }
  /* prepare thread blueprint attributes */
  CHECK(pthread_attr_init(&attr));
  //CHECK(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED));
  CHECK(pthread_attr_setschedpolicy(&attr, SCHED_RR));
  /* create mutex:
   */
  CHECK(pthread_mutexattr_init( &mutexattr ));
  CHECK(pthread_mutexattr_settype( &mutexattr, PTHREAD_MUTEX_ERRORCHECK ));
  CHECK(pthread_mutexattr_setprotocol( &mutexattr, PTHREAD_PRIO_INHERIT ));
  CHECK(pthread_mutex_init(&mutex, &mutexattr));
  /* check that we got the type that we see in the crash (34 = 32 + 2 = PI
     (priority inherit) + ERROR_CHECK) */
  printf("mutex.__data.__kind: %d\n", mutex.__data.__kind);
  ASSERT(mutex.__data.__kind == 34);
  /* Lock the mutex to avoid threads from running before we decide
   * upon their fate */
  CHECK(pthread_mutex_lock(&mutex));
  /* create a lot of candidate threads with different priorities */
  for (long i=0; i<NUM_THREADS; i++)
  {
    struct sched_param sched;
    printf("Starting thread %ld ...\n",i+1);
    sched.sched_priority = (i%99)+1;
    thread_num_to_prio[i] = sched.sched_priority;
    CHECK(pthread_attr_setschedparam(&attr, &sched));
    CHECK(pthread_create(&t1[i], &attr, helper, (void*)i));
  }
  sleep(2);
  /* Now all threads should have initialized an be waiting on the mutex */
  printf("Threads started: %d\n", num_threads_started);
  ASSERT(num_threads_started == NUM_THREADS);
  /* select a fist thread with id that has a lower byte equal to
   * 0x00 and one other thread that is likely to have the same high
   * byte but a non zero low byte. All other threads are marked
   * with 0 and will terminate immediately after unlocking the
   * mutex.*/
  {
    int found = 0;
    int num_additional_threads = 0;
    int num_threads_to_kill = 0;
    for (int i = 0; i < NUM_THREADS; ++i)
    {
      const int tid = thread_num_to_tid[i];
      if (found)
      {
        num_additional_threads += 1;
        if (num_additional_threads > NUM_THREADS_NONZERO_TO_KEEP)
        {
          thread_num_to_tid[i] = 0; // kill
          num_threads_to_kill += 1;
          num_additional_threads -= 1;
        }
      }
      else if (0 != (tid & 0xff))
      {
        thread_num_to_tid[i] = 0; // kill
        num_threads_to_kill += 1;
      }
      else
      {
        found = 1;
      }
    }
    if (0 == found)
    {
      printf("Did not find a 0x*00 thread - exiting and hoping to be restarted...\n");
      exit(0); // expect to be restarted from external shell loop
    }
    printf("Killing %d threads...\n", num_threads_to_kill);
    printf("Keep %d (non-00) threads...\n", num_additional_threads);
  }
  /* Let the races begin ... */
  CHECK(pthread_mutex_unlock(&mutex));
  /* wait for a defined time and print some statistics in between */
  for (int i = 0; i < NUM_SECONDS_TO_RUN_UNTIL_RESTART; i += 10)
  {
    sleep(10); // seconds
    printf("Main thread woke up (%d s)\n", i);
    fflush(stdout);
  }
  printf("Was running for %d seconds without detecting a problem, please restart...\n", NUM_SECONDS_TO_RUN_UNTIL_RESTART);
  return 0;
}