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


pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t wakeup = PTHREAD_COND_INITIALIZER;
pthread_cond_t done = PTHREAD_COND_INITIALIZER;
int nr_threads;
int nr_booting;
int nr_running;
int64_t burn_ns;
int64_t pace_ns;

#define NSEC_PER_SEC	1000000000L

int64_t clock_diff(struct timespec *a, struct timespec *b) {
	return (a->tv_sec - b->tv_sec) * NSEC_PER_SEC + a->tv_nsec - b->tv_nsec;
}

struct thread_data {
	struct timespec wakeup;
	struct timespec start;
	struct timespec finish;
	int64_t burn;
};

void *thread_fn(void *_data) {
	struct thread_data *data = _data;

	pthread_mutex_lock(&mutex);
	if (!--nr_booting)
		pthread_cond_signal(&done);
	while (1) {
		pthread_cond_wait(&wakeup, &mutex);
		clock_gettime(CLOCK_MONOTONIC, &data->start);
		pthread_mutex_unlock(&mutex);
		do {
			sched_yield();
			clock_gettime(CLOCK_MONOTONIC, &data->finish);
		} while (clock_diff(&data->finish, &data->start) < burn_ns);
		pthread_mutex_lock(&mutex);
		if (!--nr_running)
			pthread_cond_signal(&done);
	}
	pthread_mutex_unlock(&mutex);

	return NULL;
}

int main(int argc, char **argv) {
	nr_threads = atoi(argv[1]);
	burn_ns = atol(argv[2]);
	pace_ns = atol(argv[3]);

	pthread_t thread[nr_threads];
	struct thread_data thread_data[nr_threads];
	int nr_cpus = sysconf(_SC_NPROCESSORS_ONLN);

	nr_booting = nr_threads;
	for (int i = 0; i < nr_threads; i++)
		pthread_create(thread + i, NULL, thread_fn, thread_data + i);

	pthread_mutex_lock(&mutex);
	pthread_cond_wait(&done, &mutex);
	pthread_mutex_unlock(&mutex);

	while (1) {
		struct timespec pace_ts = {
			.tv_sec = pace_ns / NSEC_PER_SEC,
			.tv_nsec = pace_ns % NSEC_PER_SEC
		};

		clock_nanosleep(CLOCK_MONOTONIC, 0, &pace_ts, NULL);

		nr_running = nr_threads;
		pthread_cond_broadcast(&wakeup);

		pthread_mutex_lock(&mutex);
		pthread_cond_wait(&done, &mutex);
		pthread_mutex_unlock(&mutex);
	}

	return 0;
}