#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <time.h>


#define FREQ		345000000
#define HZ		250
#define NSEC_PER_SEC	1000000000LL
#define SHIFT		22

#define likely(e)	__builtin_expect(!!(e), 1)
#define unlikely(e)	__builtin_expect(!!(e), 0)

uint32_t ntp_time, ntp_update;
uint32_t cycle_update, sec;
uint64_t xtime_nsec, xtime_update, cycles_mult;
uint32_t cycles, cycles_offset;
int64_t error;

uint64_t __gettime(uint32_t offset)
{
	return xtime_nsec + (uint64_t)(cycles - cycles_offset + offset) * cycles_mult;
}

uint32_t gettime(uint32_t offset)
{
	return __gettime(offset) >> SHIFT;
}

static inline int __advanced_adjust(int sign, int64_t e, int64_t up)
{
	int adj = 0;

	printf(",^");
	//printf("<%lld,%lld", up, e);
	e += (uint64_t)ntp_update << (SHIFT - 1);
	e -= xtime_update >> 1;
	//printf(",%lld>", e);
	while (1) {
		e >>= 1;
		if (likely(sign > 0 ? e <= up : e >= up))
			return adj;
		adj++;
	}
}

#define advanced_adjust(sign, e, off, up) ({			\
	int adj = sign;						\
	e >>= 2;						\
	if (unlikely(sign > 0 ? e > up : e < up)) {		\
		adj = __advanced_adjust(sign, e, up);		\
		up <<= adj;					\
		off <<= adj;					\
		adj = sign << adj;				\
	}							\
	adj;							\
})

void simple_adjust(int64_t off)
{
	int64_t e, up;
	int adj;

	up = cycle_update;
	e = error >> (31 - SHIFT);
	printf("{%.3f", (double)error / (up << (32 - SHIFT)));
	if (e > up) {
		adj = advanced_adjust(1, e, off, up);
	} else if (e < -up) {
		up = -up;
		off = -off;
		adj = advanced_adjust(-1, e, off, up);
	} else {
		printf(",#}");
		return;
	}

	printf(",%d}", adj);
	cycles_mult += adj;
	xtime_update += up;                                                                   
	xtime_nsec -= off;
	error -= (up - off) << (32 - SHIFT);
}

void time_int(uint32_t offset)
{
	uint64_t off = cycles - cycles_offset + offset;
	uint64_t prev_time;

	prev_time = __gettime(offset);
	while (off >= cycle_update) {
		printf("*");
		off -= cycle_update;
		// After cycle_update cycles time has to match ntp_time (minus error)
		if ((uint64_t)ntp_time << 32 !=
		    ((xtime_nsec + (uint64_t)cycle_update * cycles_mult) << (32 - SHIFT)) + error)
			abort();
		cycles_offset += cycle_update;
		xtime_nsec += xtime_update;
		if (xtime_nsec >= (NSEC_PER_SEC << SHIFT)) {
			xtime_nsec -= NSEC_PER_SEC << SHIFT;
			ntp_time -= NSEC_PER_SEC;
			sec++;
			if (!(rand() % 64)) {
				int adj = (rand() % 4096) - 2048;
				ntp_update += adj;
				printf("[%d,%lld]", adj, ntp_update - NSEC_PER_SEC / HZ);
			}
		}

		ntp_time += ntp_update;

		error += ((uint64_t)ntp_update << 32) - (xtime_update << (32 - SHIFT));
	}

	simple_adjust(off);
	if ((prev_time - __gettime(offset)) % (NSEC_PER_SEC << 22))
		abort();
	if (xtime_update != cycle_update * cycles_mult)
		abort();
}

int main()
{
	uint32_t offset = 0;
	int i;

	srand(time(NULL));

	ntp_update = NSEC_PER_SEC / HZ + 300;

	cycle_update = FREQ / HZ;
	cycles_mult = (NSEC_PER_SEC << SHIFT) / FREQ;
	xtime_update = (uint64_t)cycle_update * cycles_mult;

	printf("uc: %u, cm: %lld, ut: %lld\n", cycle_update, cycles_mult, xtime_update);

	ntp_time = NSEC_PER_SEC/2;
	cycles_offset = cycles = rand();
	xtime_nsec = (uint64_t)ntp_time << SHIFT;

	ntp_time += ntp_update;
	error = ((uint64_t)ntp_update << 32) - (xtime_update << (32 - SHIFT));

	for (i = 1; 1; i++) {
		cycles += cycle_update;
		offset = rand() % cycle_update;
		//offset = cycle_update * 9 / 8;
		//offset = cycle_update - 1000;
		printf("%u(%u:%u:%u:%u:", i, sec, ntp_time,
			(uint32_t)(xtime_nsec >> SHIFT), gettime(offset));
		if (rand() % 8)
			time_int(offset);
		else
			printf("%%");
		printf(":%u): %lld,%lld,%x,%llx\n", gettime(offset),
			cycles_mult, error, cycles + offset, xtime_nsec);
	}
}