#include <sys/time.h>

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <math.h>

#include "litmus.h"
#include "common.h"


static double cputime()
{
	struct timespec ts;
	int err;
	err = clock_gettime(CLOCK_THREAD_CPUTIME_ID, &ts);
	if (err != 0)
		perror("clock_gettime");
	return (ts.tv_sec + 1E-9 * ts.tv_nsec);
}

static double wctime()
{
	struct timeval tv;
	gettimeofday(&tv, NULL);
	return (tv.tv_sec + 1E-6 * tv.tv_usec);
}

void usage(char *error) {
	fprintf(stderr, "Error: %s\n", error);
	fprintf(stderr,
		"Usage: rt_spin [-w] [-p PARTITION] [-c CLASS] [-A fp# -B fp# -W wave# -M max_wcet -P #jobs [-D #jobs]] WCET PERIOD DURATION\n"
		"       rt_spin -l\n"
		"  -A: Feedback control 'A' parameter\n"
		"  -B: Feedback control 'B' parameter\n"
		"  -W: 0 = flat; 1 = sine; 2 = triangle; 3 = box; 4 = sawtooth; 5 = reverse sawtooth\n"
		"  -M: max wcet (WCET is assumed min wcet)\n"
		"  -P: period of execution behavior wave\n"
		"  -D: Number of jobs wave should hold at extrema points (default = 0)\n");
	exit(1);
}

#define NUMS 4096
static int num[NUMS];
static double loop_length = 1.0;
static char* progname;

static int loop_once(void)
{
	int i, j = 0;
	for (i = 0; i < NUMS; i++)
		j += num[i]++;
	return j;
}

static int loop_for(double exec_time)
{
	double t = 0;
	int tmp = 0;	
/*	while (t + loop_length < exec_time) {
		tmp += loop_once();
		t += loop_length;
	}
*/
	double start = cputime();
	double now = cputime();
	while (now + loop_length < start + exec_time) {
		tmp += loop_once();
		t += loop_length;
		now = cputime();
	}

	return tmp;
}

static void fine_tune(double interval)
{
	double start, end, delta;

	start = wctime();
	loop_for(interval);
	end = wctime();
	delta = (end - start - interval) / interval;
	if (delta != 0)
		loop_length = loop_length / (1 - delta);
}

static void configure_loop(void)
{
	double start;

	/* prime cache */
	loop_once();
	loop_once();
	loop_once();

	/* measure */
	start = wctime();
	loop_once(); /* hope we didn't get preempted  */
	loop_length = wctime();
	loop_length -= start;

	/* fine tune */
	fine_tune(0.1);
	fine_tune(0.1);
	fine_tune(0.1);
}

static void show_loop_length(void)
{
	printf("%s/%d: loop_length=%f (%ldus)\n",
	       progname, getpid(), loop_length,
	       (long) (loop_length * 1000000));
}

static void debug_delay_loop(void)
{
	double start, end, delay;
	show_loop_length();
	while (1) {
		for (delay = 0.5; delay > 0.01; delay -= 0.01) {
			start = wctime();
			loop_for(delay);
			end = wctime();
			printf("%6.4fs: looped for %10.8fs, delta=%11.8fs, error=%7.4f%%\n",
			       delay,
			       end - start,
			       end - start - delay,
			       100 * (end - start - delay) / delay);
		}
	}
}

static int job(double exec_time)
{
	loop_for(exec_time);
	sleep_next_period();
	return 0;
}

fp_t float_to_frac(double f)
{
	fpbuf_t num;
	fpbuf_t denom = 1000; /* express in 1000ths */

	num = (fpbuf_t)f * denom;
	if(f != floor(f))
	{
		num += (fpbuf_t)(f - floor(f)) * denom;
	}

	return(_frac(num, denom));
}

#define OPTSTR "p:c:wld:veA:B:W:M:P:D:"

struct wave_data;
typedef double(*wave_func)(int, struct wave_data*);

struct wave_data
{
	double min_wcet_ms;
	double max_wcet_ms;
	double amplitude;
	int period;
	int extrema_delay;
	int min_point;
	int max_point;
	wave_func f;
};

double flat_wave(int count, struct wave_data* w)
{
	return(w->min_wcet_ms);
}

double sin_wave(int count, struct wave_data* w)
{
	double step = count % w->period;
	double exe = w->min_wcet_ms + w->amplitude * sin(M_PI*step/w->period);
	return(exe);
}

double tri_wave(int count, struct wave_data* w)
{
	double step = count % w->period;
	double v = 2.0 * (step/w->period - floor(step/w->period + 0.5));
	double exe = w->min_wcet_ms + w->amplitude * fabs(v);
	return(exe);
}

double box_wave(int count, struct wave_data* w)
{
	double step = count % w->period;
	double exe = w->min_wcet_ms + w->amplitude *((step/w->period < 0.5) ? 0 : 1);
	return(exe);
}

double saw_wave(int count, struct wave_data* w)
{
	double step = (count % w->period)/2.0;
	double v = 2.0 * (step/w->period - floor(step/w->period + 0.5));
	double exe;
	if(v < 0) v += 1.0;
	exe = w->min_wcet_ms + w->amplitude * v;
	return(exe);
}

double rsaw_wave(int count, struct wave_data* w)
{
	double step = (count % w->period)/2.0;
	double v = 2.0 * (step/w->period - floor(step/w->period + 0.5));
	double exe;
	if(v < 0) v += 1.0;
	exe = w->max_wcet_ms - w->amplitude * v;
	return(exe);
}

double get_execution_time(int count, struct wave_data* w)
{
	static int skips = 0;
	static int delay_count = 0;

	if(w->extrema_delay)
	{
		int at_max = (((count - skips) % w->period) == w->max_point);
		int at_min = (((count - skips) % w->period) == w->min_point);

		if((at_max || at_min) && delay_count++ < w->extrema_delay)
		{
			++skips;
			return((at_max) ? w->max_wcet_ms : w->min_wcet_ms);
		}
		else
		{
			delay_count = 0;
		}
	}

	return(w->f(count - skips, w));
}

int main(int argc, char** argv) 
{
	int ret;
	lt_t wcet;
	lt_t period;
	double wcet_ms, period_ms;
	int migrate = 0;
	int cpu = 0;
	int opt;
	int wait = 0;
	int test_loop = 0;
	int skip_config = 0;
	int verbose = 0;
	int want_enforcement = 0;
	double duration, start;
	task_class_t class = RT_CLASS_HARD;

	fp_t A = LITMUS_FP_ZERO, B = LITMUS_FP_ZERO;
	struct wave_data wave = {
			.min_wcet_ms = 0.0,
			.max_wcet_ms = 0.0,
			.amplitude = 0.0,
			.period = 0,
			.extrema_delay = 0,
			.max_point = 0,
			.min_point = 0,
			.f = NULL
	};

	progname = argv[0];

	while ((opt = getopt(argc, argv, OPTSTR)) != -1) {
		switch (opt) {
		case 'w':
			wait = 1;
			break;
		case 'p':
			cpu = atoi(optarg);
			migrate = 1;
			break;
		case 'c':
			class = str2class(optarg);
			if (class == -1)
				usage("Unknown task class.");
			break;
		case 'e':
			want_enforcement = 1;
			break;
		case 'l':
			test_loop = 1;
			break;
		case 'd':
			/* manually configure delay per loop iteration 
			 * unit: microseconds */
			loop_length = atof(optarg) / 1000000;
			skip_config = 1;
			break;
		case 'v':
			verbose = 1;
			break;
		case 'A':
			A = float_to_frac(atof(optarg));
			break;
		case 'B':
			B = float_to_frac(atof(optarg));
			break;
		case 'W':
			switch(atoi(optarg))
			{
				case 0:
					wave.f = flat_wave;
					break;
				case 1:
					wave.f = sin_wave;
					break;
				case 2:
					wave.f = tri_wave;
					break;
				case 3:
					wave.f = box_wave;
					break;
				case 4:
					wave.f = saw_wave;
					break;
				case 5:
					wave.f = rsaw_wave;
					break;
				default:
					usage("Bad wave identifier.");
					break;
			}
			break;
		case 'M':
			wave.max_wcet_ms = atof(optarg);
			break;
		case 'P':
			wave.period = atoi(optarg);
			if(wave.period <= 0)
				usage("Execution behavior period must be a positive number.");
			break;
		case 'D':
			wave.extrema_delay = atoi(optarg);
			break;
		case ':':
			usage("Argument missing.");
			break;
		case '?':
		default:
			usage("Bad argument.");
			break;
		}
	}


	if (!skip_config)
		configure_loop();

	if (test_loop) {
		debug_delay_loop();
		return 0;
	}

	if (argc - optind < 3)
		usage("Arguments missing.");
	wcet_ms   = atof(argv[optind + 0]);
	period_ms = atof(argv[optind + 1]);
	duration  = atof(argv[optind + 2]);
	wcet   = wcet_ms * __NS_PER_MS;
	period = period_ms * __NS_PER_MS;
	if (wcet <= 0)
		usage("The worst-case execution time must be a "
		      "positive number.");
	if (period <= 0)
		usage("The period must be a positive number.");
	if (wcet > period) {
		usage("The worst-case execution time must not "
		      "exceed the period.");
	}

	if(wave.f != NULL)
	{
		wave.min_wcet_ms = wcet_ms;
		wave.amplitude = wave.max_wcet_ms - wave.min_wcet_ms;
		if(wave.amplitude < 0.0)
		{
			usage("The max wcet must be greater than min wcet.");
		}

		if(wave.f == flat_wave)
		{
			wave.max_point = 0;
			wave.min_point = 0;
		}
		else if((wave.f == sin_wave) || (wave.f == tri_wave) || (wave.f == box_wave))
		{
			wave.max_point = wave.period/2;
			wave.min_point = 0;
		}
		else if(wave.f == saw_wave)
		{
			wave.max_point = wave.period-1;
			wave.min_point = 0;
		}
		else if(wave.f == rsaw_wave)
		{
			wave.max_point = 0;
			wave.min_point = wave.period-1;
		}

		if((wave.f == flat_wave) || (wave.f == box_wave))
		{
			wave.extrema_delay = 0;
		}
	}

	if (migrate) {
		ret = be_migrate_to(cpu);
		if (ret < 0)
			bail_out("could not migrate to target partition");
	}

	ret = sporadic_task_ns(wcet, period, 0,
				   A, B,
				   cpu, class,
			       want_enforcement ? PRECISE_ENFORCEMENT
			                        : NO_ENFORCEMENT,
			       migrate);

	if (ret < 0)
		bail_out("could not setup rt task params");

	if (verbose)
		show_loop_length();

	init_litmus();

	ret = task_mode(LITMUS_RT_TASK);
	if (ret != 0)
		bail_out("could not become RT task");

	if (wait) {
		ret = wait_for_ts_release();
		if (ret != 0)
			bail_out("wait_for_ts_release()");
	}

	start = wctime();

	int count = 0;
	while (start + duration > wctime())
	{
		if(wave.f == NULL)
		{
			job(wcet_ms * 0.0009); /* 90% wcet, in seconds */
		}
		else
		{
			job(get_execution_time(count++, &wave) * 0.0009);
		}
	}

	return 0;
}