#include <pthread.h>
#include <time.h>
#include <unistd.h>
#include <sys/syscall.h>
#include <linux/futex.h>
#include <immintrin.h>

#define SPIN_THRESHOLD_NS 500000	// NOTE(peter): 500µs spin threshold for Linux

struct timer_handle {
	uint64_t interval_ns;
	struct timespec next_deadline;
	uint32_t running;
	pthread_t timer_thread;

	volatile int futex_word;

#ifdef TIMER_DEBUG
	struct timespec last_wait_start;
#endif
};

static void timespec_add_ns(struct timespec *ts, uint64_t ns) {
	ts->tv_nsec += ns;
	while(ts->tv_nsec >= 1000000000L) {
		ts->tv_nsec -= 1000000000L;
		ts->tv_sec++;
	}
}

static int64_t timespec_diff_ns(struct timespec *a, struct timespec *b) {
	int64_t sec = a->tv_sec - b->tv_sec;
	int64_t nsec = a->tv_nsec - b->tv_nsec;

	if(nsec < 0) {
		nsec += 1000000000L;
		sec -= 1;
	}

	return sec * 1000000000LL + nsec;
}

static int futex_wait(volatile int *addr, int val) {
	return syscall(SYS_futex, addr, FUTEX_WAIT_PRIVATE, val, 0, 0, 0);
}

static int futex_wake(volatile int *addr) {
	return syscall(SYS_futex, addr, FUTEX_WAKE_PRIVATE, 1, 0, 0, 0);
}

static void *timer_thread_func(void *arg) {
	struct timer_handle *t = (struct timer_handle *)arg;

	while(t->running) {
		struct timespec now;
		clock_gettime(CLOCK_MONOTONIC_RAW, &now);

#ifdef TIMER_DEBUG
		int64_t remaining_after_sleep_ns = -1;
#endif

		int64_t diff_ns = timespec_diff_ns(&t->next_deadline, &now);
		if(diff_ns > SPIN_THRESHOLD_NS) {
			struct timespec sleep_time;
			uint64_t sleep_ns = diff_ns - SPIN_THRESHOLD_NS;
			sleep_time.tv_sec = sleep_ns / 1000000000;
			sleep_time.tv_nsec = sleep_ns % 1000000000;
			nanosleep(&sleep_time, 0);
#ifdef TIMER_DEBUG
			clock_gettime(CLOCK_MONOTONIC_RAW, &now);
			remaining_after_sleep_ns = timespec_diff_ns(&t->next_deadline, &now);
#endif
		}

		while(clock_gettime(CLOCK_MONOTONIC_RAW, &now), timespec_diff_ns(&t->next_deadline, &now) > 0) {
			_mm_pause();
		}

		t->futex_word = 1;
		futex_wake(&t->futex_word);

#ifdef TIMER_DEBUG
		if(t->last_wait_start.tv_sec) {
			int64_t overshoot_ns = timespec_diff_ns(&now, &t->next_deadline);
			if(overshoot_ns < 0) overshoot_ns = 0;

			if(remaining_after_sleep_ns >= 0) {
				DEBUG_PRINT("[DEBUG] Woke up with %ld ns left. Overshoot: %5ld ns\n", remaining_after_sleep_ns, overshoot_ns);
			} else {
				DEBUG_PRINT("[DEBUG] No sleep. Overshoot: %ld ns\n", overshoot_ns);
			}
		}
		t->last_wait_start = now;
#endif

		timespec_add_ns(&t->next_deadline, t->interval_ns);
	}

	return 0;
}

static struct timer_handle *timer_new(uint64_t interval_ns) {
	struct timer_handle *t = calloc(1, sizeof(struct timer_handle));
	if(!t) {
		printf("ERROR: Could not allocate a timer!\n");
		exit(1);
	}

	t->interval_ns = interval_ns;
	clock_gettime(CLOCK_MONOTONIC_RAW, &t->next_deadline);
	timespec_add_ns(&t->next_deadline, interval_ns);
	t->running = 1;
	t->futex_word = 0;

#ifdef TIMER_DEBUG
	t->last_wait_start.tv_sec = 0;
	t->last_wait_start.tv_nsec = 0;
#endif

	pthread_create(&t->timer_thread, 0, timer_thread_func, t);

	return t;
}

static void timer_init() {
}

static void timer_shutdown() {
}

static uint32_t timer_wait(struct timer_handle *t) {
	futex_wait(&t->futex_word, 0);
	t->futex_word = 0;
	return 1;
}

static void timer_destroy(struct timer_handle *t) {
	t->running = 0;

	t->futex_word = 1;
	futex_wake(&t->futex_word);
	pthread_join(t->timer_thread, 0);
	free(t);
}