#define _GNU_SOURCE
#include <pthread.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <sched.h>

struct timeval start, end;
unsigned long long i, cnt;
long utime, seconds, useconds;

void enable_early() {
	system("echo 1 > /sys/devices/system/cpu/cpufreq/ondemand/early_demand");
        system("rmmod cpufreq_stats");
        system("modprobe cpufreq_stats");
}

void disable_early() {
        system("echo 0 > /sys/devices/system/cpu/cpufreq/ondemand/early_demand");
	system("rmmod cpufreq_stats");
	system("modprobe cpufreq_stats");
}

void calibrate()
{
	sleep(1);

	gettimeofday(&start, NULL);

	for (i = 0; i < 1000000000; i++);

	gettimeofday(&end, NULL);
	seconds = end.tv_sec - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	utime = seconds * 1000000 + useconds;
	
	printf("Calibrating\n");
	printf("Elapsed time: %ld microseconds\n", utime);

	/* find the counter for 10ms */
	cnt = i * 10000 / utime;

	printf("cnt: %ld\n", cnt);
}

long do_bench()
{
	gettimeofday(&start, NULL);

	for (i = 0; i < cnt; i++);

	gettimeofday(&end, NULL);
	seconds = end.tv_sec - start.tv_sec;
	useconds = end.tv_usec - start.tv_usec;
	utime = seconds * 1000000 + useconds;
	
	printf("Elapsed time: %ld microseconds\n", utime);

	return utime;
}

void benchmark()
{
	const int iter = 100;
	long total_off = 0;
	long total_on = 0;
	double diff_perc;
	unsigned int i;

	/* calibrate(); */
	cnt = 1000000;

	disable_early();
	sleep(1);
	do_bench(); /* do a first benchmark but do not count in total */	
	sleep(5);

	for (i = 0; i < iter; i++) {
		total_off += do_bench();
		
		usleep(500000);
	}
	total_off /= iter;

	system("cat /sys/devices/system/cpu/cpu2/cpufreq/stats/time_in_state");

	enable_early();
	sleep(1);
	do_bench(); /* do a first benchmark but do not count in total */
	sleep(5);

	for (i = 0; i < iter; i++) {
                total_on += do_bench();

                usleep(500000);
        }
        total_on /= iter;

	system("cat /sys/devices/system/cpu/cpu2/cpufreq/stats/time_in_state");
	diff_perc = (total_off - total_on) * 100;
	diff_perc /= total_off;

	printf("early_demand off: %ld us\n", total_off);
        printf("early_demand on: %ld us\n", total_on);
	printf("diff: %f\n", diff_perc);
}

main ()
{
	int i;

	cpu_set_t  mask;
	CPU_ZERO(&mask);
	CPU_SET(2, &mask);
	sched_setaffinity(0, sizeof(mask), &mask);

	printf("Starting benchmark\n");
	
	for (i = 0; i < 5; i++) {
		printf("run %i\n", i);
		benchmark();
	}
}