#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <pthread.h>
#include <dispatch/dispatch.h>

// https://opensource.apple.com/source/xnu/xnu-7195.50.7.100.1/bsd/sys/ulock.h.auto.html
extern int __ulock_wait(uint32_t op, void *addr, uint64_t value, uint32_t timeout);
extern int __ulock_wake(uint32_t op, void *addr, uint64_t wake_value);
#define UL_COMPARE_AND_WAIT 1
#define ULF_WAKE_ALL 0x00000100

#define N 10000
#define DELAY_MS 50

const char *ExperimentName;
void(*ExperimentWait)(void);
void(*ExperimentWake)(void);
double StartTime;
double Dt[N];
_Atomic int BarrierA;
_Atomic int BarrierB;

double Time(void) {
	struct timespec ts;
	timespec_get(&ts, TIME_UTC);
	return (double)ts.tv_sec + ts.tv_nsec / 1e9;
}

void *ThreadA(void *parameter)
{
	(void)parameter;
	for (int i = 0; i < N; ++i)
	{
		++BarrierA;
		while (BarrierB != i + 1);

		ExperimentWait();

		double endTime = Time();

		double dt = endTime - StartTime;
		Dt[i] = dt;
	}
	return NULL;
}
void *ThreadB(void *parameter)
{
	(void)parameter;
	for (int i = 0; i < N; ++i)
	{
		++BarrierB;
		while (BarrierA != i + 1);

		struct timespec ts = {
			.tv_sec = 0,
			.tv_nsec = DELAY_MS * 1000000,
		};
		struct timespec rem;
		nanosleep(&ts, &rem);

		StartTime = Time();
		ExperimentWake();
	}
	return NULL;
}
int Compare(const void *a, const void *b)
{
	double A = *(double *)a;
	double B = *(double *)b;
	if (A < B)
		return -1;
	else if (A > B)
		return +1;
	else
		return 0;
}
void Run(void)
{
	BarrierA = 0;
	BarrierB = 0;

	pthread_t a, b;
	pthread_create(&a, NULL, ThreadA, NULL);
	pthread_create(&b, NULL, ThreadB, NULL);
	pthread_join(a, NULL);
	pthread_join(b, NULL);

	qsort(Dt, N, sizeof Dt[0], Compare);
	double avg = 0;
	double min = 1e99;
	double max = 0;
	double med = Dt[N / 2];
	for (int i = 0; i < N; ++i) {
		avg += Dt[i];
		if (Dt[i] < min) min = Dt[i];
		if (Dt[i] > max) max = Dt[i];
	}
	avg /= N;

	double sdev = 0;
	for (int i = 0; i < N; ++i) {
		double dev = Dt[i] - avg;
		sdev += dev * dev;
	}
	sdev = sqrt(sdev / N);

	printf("%s avg: %.2f  sdev: %.2f  min: %.1f  max: %.1f  median: %.1f us\n",
		ExperimentName, avg * 1e6, sdev * 1e6, min * 1e6, max * 1e6, med * 1e6);
}

_Atomic long Spin;
void SpinWait(void)
{
	while (!Spin);
	Spin = 0;
}
void SpinWake(void)
{
	Spin = 1;
}

_Atomic int64_t Atomic;
void AtomicWait(void)
{
	while (!Atomic)
		__ulock_wait(UL_COMPARE_AND_WAIT, &Atomic, 0, 0xFFFFFFFF);
	Atomic = 0;
}
void AtomicWake(void)
{
	Atomic = 1;
	__ulock_wake(UL_COMPARE_AND_WAIT | ULF_WAKE_ALL, &Atomic, 1);
}

dispatch_semaphore_t Semaphore;
void SemaphoreWait(void)
{
	dispatch_semaphore_wait(Semaphore, DISPATCH_TIME_FOREVER);
}
void SemaphoreWake(void)
{
	dispatch_semaphore_signal(Semaphore);
}

_Atomic long Sleep;
void SleepWait(void)
{
	while (!Sleep) {
		struct timespec ts = {
			.tv_sec = 0,
			.tv_nsec = 1000,
		};
		struct timespec rem;
		nanosleep(&ts, &rem);
	}
	Sleep = 0;
}
void SleepWake(void)
{
	Sleep = 1;
}

int main(void)
{
	Semaphore = dispatch_semaphore_create(0);

	ExperimentName = "Spin";
	ExperimentWait = SpinWait;
	ExperimentWake = SpinWake;
	Run();

	ExperimentName = "Atomic";
	ExperimentWait = AtomicWait;
	ExperimentWake = AtomicWake;
	Run();

	ExperimentName = "Semaphore";
	ExperimentWait = SemaphoreWait;
	ExperimentWake = SemaphoreWake;
	Run();

	ExperimentName = "Sleep";
	ExperimentWait = SleepWait;
	ExperimentWake = SleepWake;
	Run();
}