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

#if 1

typedef int64_t kclock_t;

#define MSEC		1000000

#define TICK_SLICE	((kclock_t)10*MSEC)
#define MAX_SLICE	((kclock_t)100*MSEC)

#define MSHIFT		16

#define MSCALE		1000000

#else

typedef int32_t kclock_t;

#define TICK_SLICE	10
#define MAX_SLICE	100

#define MSHIFT		0

#define MSCALE		1

#endif

#define MAX_TIMESUM	((kclock_t)1 << (30 + MSHIFT))

#define MAX_TASKS	20

struct task {
	unsigned int weight, weight_inv;
	int active, prio, weight_shift;

	kclock_t time;
	kclock_t time_norm, time_key, req_weight_inv;

	kclock_t req_size;
} task[MAX_TASKS]  = {
	{ .prio = 10, },
	{ .prio = 20, },
	{ .prio = 30, },
};

#if 1
/* 2^16*1.25^(i-39) */
unsigned int weight_inv_table[40] = {
	11, 14, 17, 21, 27, 33, 42, 52, 65, 81,
	101, 127, 158, 198, 248, 309, 387, 484, 604, 756,
	944, 1181, 1476, 1845, 2306, 2882, 3603, 4504, 5629, 7037,
	8796, 10995, 13744, 17180, 21475, 26844, 33554, 41943, 52429, 65536
};

/* 2^20/weight_inv_table[i] */
unsigned int weight_table[40] = {
	95325, 74898, 61681, 49932, 38836, 31775, 24966, 20165, 16132, 12945,
	10382, 8257, 6637, 5296, 4228, 3393, 2709, 2166, 1736, 1387,
	1111, 888, 710, 568, 455, 364, 291, 233, 186, 149,
	119, 95, 76, 61, 49, 39, 31, 25, 20, 16
};

/* l(1.25^(39-i))/l(2) */
unsigned int weight_shift_table[40] = {
	13, 12, 12, 12, 11, 11, 11, 10, 10, 10,
	9, 9, 9, 8, 8, 8, 7, 7, 7, 6,
	6, 6, 5, 5, 5, 5, 4, 4, 4, 3,
	3, 3, 2, 2, 2, 1, 1, 1, 0, 0
};
#else
/* 2^16*e(l(2)*((i-39)/5)) */
unsigned int weight_inv_table[40] = {
	294, 338, 388, 446, 512, 588, 676, 776, 891, 1024,
	1176, 1351, 1552, 1783, 2048, 2353, 2702, 3104, 3566, 4096,
	4705, 5405, 6208, 7132, 8192, 9410, 10809, 12417, 14263, 16384,
	18820, 21619, 24834, 28526, 32768, 37641, 43238, 49667, 57052, 65536
};

/* 2^20/weight_inv_table[i] */
unsigned int weight_table[40] = {
	3567, 3102, 2703, 2351, 2048, 1783, 1551, 1351, 1177, 1024,
	892, 776, 676, 588, 512, 446, 388, 338, 294, 256,
	223, 194, 169, 147, 128, 111, 97, 84, 74, 64,
	56, 49, 42, 37, 32, 28, 24, 21, 18, 16
};

/* (39-i)/5 */
unsigned int weight_shift_table[40] = {
	8, 8, 7, 7, 7, 7, 7, 6, 6, 6,
	6, 6, 5, 5, 5, 5, 5, 4, 4, 4,
	4, 4, 3, 3, 3, 3, 3, 2, 2, 2,
	2, 2, 1, 1, 1, 1, 1, 0, 0, 0
};
#endif


#define TASK_NORM(tsk, time)	((time) * task[tsk].weight_inv)

/* yes, this can be improved... */
#define KCLOCK_MIN(x, y)	((kclock_t)((x) - (y)) < 0 ? (x) : (y))
#define KCLOCK_MAX(x, y)	((kclock_t)((x) - (y)) > 0 ? (x) : (y))

kclock_t time_norm_base, time_norm_inc, run_start, run_end;
kclock_t time_sum_max, time_sum_off;
int current, weight_sum, inc_shift;


void task_enqueue(int t);
void task_dequeue(int t);


int task_next(void)
{
	int i, next;
	static int last = -1;

	next = -1;
	/* this should be a sorted tree... */
	for (i = 0; i < MAX_TASKS; i++) {
		if (i == current)
			continue;
		if (!task[i].active && task[i].weight) {
			if (!(rand() % 20))
				task_enqueue(i);
		}
		if (!task[i].active)
			continue;
		if (next < 0 || (kclock_t)(task[i].time_key - task[next].time_key) < 0)
			next = i;
	}

	if (next < 0)
		return -1;

	/* using task[current].time_norm this can be used to calculate
	 * the real end time to e.g. program a timer.
	 */
	run_end = task[next].time_norm + task[next].req_weight_inv;
	if (current >= 0)
		run_end = KCLOCK_MIN(run_end, KCLOCK_MAX(task[next].time_norm, 
							 run_start + task[current].req_weight_inv));
	if (last != next) {
		printf("next: %u,%f\n", next, current < 0 ? 0 :
			(double)(run_end - task[current].time_norm) * task[current].weight / (1 << 20) / MSCALE);
		last = next;
	}
	return next;
}

void task_update_key(int t)
{
	task[t].time_key = task[t].time_norm + task[t].req_weight_inv / 2;
}

void task_switch(int next)
{
	if (next < 0)
		return;

	/* every weight_sum*inc times, time_norm_base is updated by inc */
	while (time_sum_off >= time_sum_max) {
		printf("---\n");
		time_sum_off -= time_sum_max;
		time_norm_base += time_norm_inc;
	}

	if (current >= 0)
		task_update_key(current);
	current = next;
	run_start = task[current].time_norm;
	/* this sets run_end and can be used to program a timer */
	task_next();
}

void timer_tick(void)
{
	kclock_t time_sum;
	double time_sum2;
	int i, weigth_sum;

	if (current >= 0) {
		task[current].time += TICK_SLICE;
		task[current].time_norm += TASK_NORM(current, TICK_SLICE);
		time_sum_off += TASK_NORM(current, TICK_SLICE) << task[current].weight_shift;
		//printf("%Lx,%Lx,%Lx\n", (long long)time_sum_off, (long long)time_sum_max, (long long)time_norm_inc);
	}

	printf("%d", current);
	time_sum = weigth_sum = 0;
	time_sum2 = 0;
	for (i = 0; i < MAX_TASKS; i++) {
		if (!task[i].active) {
			if (task[i].weight)
				printf("\t%3u/%u: -\t", i, task[i].weight);
			continue;
		}
		weigth_sum += task[i].weight;
		time_sum2 += (double)task[i].time_norm * task[i].weight;
		time_sum += (task[i].time_norm - time_norm_base) << task[i].weight_shift;
		printf("\t%3u/%.3f:%5f/%-7f(%-5f)", i, (double)task[i].weight / (1 << 4), (double)task[i].time / MSCALE,
			(double)task[i].time_norm / (1 << 16) / MSCALE,
			(double)(kclock_t)(time_norm_base + time_sum_off / weight_sum - task[i].time_norm) * task[i].weight / (1 << 20) / MSCALE);
	}
	if (time_sum != time_sum_off)
		abort();
	printf("\t| %f(%f)\n", (time_norm_base + (double)time_sum_off / weight_sum) / (1 << 16) / MSCALE,
		((time_sum2 / weigth_sum) - (time_norm_base + (double)time_sum_off / weight_sum)) / (1 << 16) / MSCALE);
}

void task_enqueue(int t)
{
	kclock_t min_time;

	if (!weight_sum)
		goto done;

	min_time = time_norm_base - task[t].req_weight_inv;
	min_time += (kclock_t)((int32_t)(time_sum_off >> MSHIFT) / weight_sum) << MSHIFT;

	if ((kclock_t)((task[t].time_norm) - min_time) < 0)
		task[t].time_norm = min_time;
done:
	task[t].active = 1;
	task_update_key(t);

	weight_sum += 1 << task[t].weight_shift;
	/* keep time_sum_max below MAX_TIMESUM
	 * which allows to easily change it to a lower resolution 32bit value
	 */
	if (inc_shift < task[t].weight_shift) {
		time_norm_inc >>= task[t].weight_shift - inc_shift;
		time_sum_max >>= task[t].weight_shift - inc_shift;
		inc_shift = task[t].weight_shift;
		//printf("w++(%d):%Lx,%Lx\n", inc_shift, (long long)time_norm_inc, (long long)time_sum_max);
	}
	time_sum_max += time_norm_inc << task[t].weight_shift;
	while (time_sum_max >= MAX_TIMESUM) {
		time_norm_inc >>= 1;
		time_sum_max >>= 1;
		inc_shift++;
		//printf("w+(%d):%Lx,%Lx\n", inc_shift, (long long)time_norm_inc, (long long)time_sum_max);
	}
	time_sum_off += (task[t].time_norm - time_norm_base) << task[t].weight_shift;
	if (time_sum_off >= time_sum_max) {
		time_sum_off -= time_sum_max;
		time_norm_base += time_norm_inc;
	}
}

void task_dequeue(int t)
{
	task[t].active = 0;
	weight_sum -= 1 << task[t].weight_shift;
	if (!weight_sum)
		current = -1;
	time_sum_max -= time_norm_inc << task[t].weight_shift;
	if (time_sum_max) {
		while (time_sum_max < (MAX_TIMESUM >> 1)) {
			time_norm_inc <<= 1;
			time_sum_max <<= 1;
			inc_shift--;
			//printf("w-(%d):%Lx,%Lx\n", inc_shift, (long long)time_norm_inc, (long long)time_sum_max);
		}
	} else {
		time_norm_inc = MAX_TIMESUM >> 1;
		inc_shift = 0;
	}
	time_sum_off -= (task[t].time_norm - time_norm_base) << task[t].weight_shift;
	if (time_sum_off < 0) {
		time_sum_off += time_sum_max;
		time_norm_base -= time_norm_inc;
	}
}

int main(void)
{
	int i, next;

	time_norm_inc = MAX_TIMESUM >> 1;
	weight_sum = 0;
	for (i = 0; i < MAX_TASKS; i++) {
		if (task[i].prio < 1 || task[i].prio > 40)
			continue;
		task[i].weight = weight_table[task[i].prio - 1];
		task[i].weight_inv = weight_inv_table[task[i].prio - 1];
		task[i].weight_shift = weight_shift_table[task[i].prio - 1];
		if (task[i].req_size)
			task[i].req_weight_inv = task[i].req_size * task[i].weight_inv;
		else
			task[i].req_weight_inv = MAX_SLICE * task[i].weight_inv;
		task_enqueue(i);
	}

	current = -1;
	for (;; timer_tick()) {
		next = task_next();

		if (current < 0) {
			task_switch(next);
			continue;
		}

#if 1
		if (!(rand() % 100)) {
			task_dequeue(current);
			task_switch(next);
			continue;
		}
#endif

		if ((kclock_t)(task[current].time_norm - run_end) >= 0)
			task_switch(next);
	}
}