#include <stdio.h>
#include <pthread.h>

#include <sys/time.h>
#include <linux/timex.h>

#include "utils.h"

#define HZ_ACCURACY 3

#define NUM_REC_JITTER 10000
#define WARMUP_ITERATIONS 500


// from Dr. Baker's bisection.c
/* This example is very sloppy about checking and recovering
   from failed system calls.  Essentially, we are assuming
   everything goes right.  One sloppy detail is that we are
   not distinguishing functions that return -1 for failure
   and set errno from functions that return the errno value
   for failure instead of setting errno.  Another sloppy
   detail is that with Linux threads exiting the process
   does not kill off all the threads; a bunch of threads
   may be left running.
 */
#define CHECK(CALL) if (CALL) { perror (#CALL); exit (-1); } 

struct time_jitter {
	cycles_t theo_start;
	cycles_t actual_start;	

	struct timespec clock_theo_start;
	struct timespec clock_actual_start;
};


/********** globals ***********/
double secs_per_cycle;
double cpu_hz;
struct time_jitter rec_jitter[NUM_REC_JITTER];



cycles_t
cvt_microsecs_cycles(double microsecs) {
	cycles_t rtn_val = 0;
	rtn_val = (1.0L/secs_per_cycle)*(microsecs/1.0E6L);	
	return rtn_val;
}


double cvt_cycles_microsecs(cycles_t cycles) {
	double rtn_val = 0;
	rtn_val = secs_per_cycle*cycles;
	rtn_val = rtn_val*1E6L;
	return rtn_val;		
}

void print_pthread_attr_priority(pthread_attr_t* attr) {
	struct sched_param sched;

	pthread_attr_getschedparam(attr, &sched);
	printf("priority: %d\n", sched.sched_priority);
}



void *test_period() {
	int i;
	int ret;
	double nano_period;
	cycles_t cycle_period, cyc_prev_start;
	struct timespec timeout;

	nano_period = 500000;
	
	printf("period(nsec): %.16f\n", nano_period);
	cycles_t cyc_actual_start;

	printf("test will take approx: %u sec(s)\n", (unsigned int)((nano_period*NUM_REC_JITTER)/1.0E9) );
	
	// get our initial time values	
	clock_gettime(CLOCK_REALTIME,&timeout);

	// we want to get the theoretical time the pthread should continue
	cycle_period = cvt_microsecs_cycles(nano_period/1.0E3L);			
	printf("period(cycles): %llu\n", cycle_period);

	// get our initial time value
	clock_gettime(CLOCK_REALTIME,&timeout);
        cyc_actual_start = get_cycles();

	timeout.tv_sec = 0;
	timeout.tv_nsec = nano_period;

	for (i=0; i<NUM_REC_JITTER; ++i) {
		ret = nanosleep(&timeout, NULL);
		// we are started, what time did we start?
                cyc_prev_start = cyc_actual_start;
		cyc_actual_start = get_cycles();

		if (ret) {
			printf ("ERROR: nanosleep returned : %d\n", ret);
		}

		rec_jitter[i].theo_start   = cyc_prev_start + cycle_period;
		rec_jitter[i].actual_start = cyc_actual_start;
	}

}

void init_pthread_attr(pthread_attr_t* attr) {
	struct sched_param p_sched_param;

	CHECK( pthread_attr_init(attr) );
	// set the threads to be in competition with all other threads on the
	// system not just this process
	CHECK( pthread_attr_setscope(attr, PTHREAD_SCOPE_SYSTEM) );
	CHECK( pthread_attr_setschedpolicy(attr, SCHED_FIFO) );
	CHECK( pthread_attr_getschedparam(attr, &p_sched_param) );
	CHECK( pthread_attr_setinheritsched(attr,  PTHREAD_EXPLICIT_SCHED) );
	p_sched_param.sched_priority = sched_get_priority_max(SCHED_FIFO);
	if (p_sched_param.sched_priority != sched_get_priority_max(SCHED_FIFO)) {
			  printf("ERROR: could not set sched_priority to max priority");
	}

	CHECK( pthread_attr_setschedparam(attr, &p_sched_param) );
}

void output_results() {
	int i;
	struct timespec theo_start, act_start, diff_start;
	double cyc_theo_start, cyc_act_start;
	double yaxis = 5;
	double diff_secs;
	int put_bucket;

	int pos_num_buckets = 10000;
	int neg_num_buckets = 1000;
	int total_buckets = pos_num_buckets + neg_num_buckets;
	int bucket[total_buckets];

	FILE *fp;
	char *OUTPUT_FILE = "bucket.dat";

	int neg = 0;


	for (i=0; i<total_buckets; ++i) {
		bucket[i] = 0;
	}
	for (i=0; i<NUM_REC_JITTER; ++i) {
		neg = 0;
		// TODO: cycle counter can wrap around need to take this into account
		// printf ("%llu:%llu\n", rec_jitter[i].theo_start, rec_jitter[i].actual_start);
		cyc_theo_start = cvt_cycles_microsecs(rec_jitter[i].theo_start);
		cyc_act_start  = cvt_cycles_microsecs(rec_jitter[i].actual_start);
		if (cyc_act_start < cyc_theo_start) {
			neg = 1;
			cyc_act_start = cyc_theo_start - cyc_act_start;
		} else {
			cyc_act_start -= cyc_theo_start;
		}

		theo_start = rec_jitter[i].clock_theo_start;
		act_start = rec_jitter[i].clock_actual_start;
		diff_start = timespec_sub(act_start, theo_start);
		
		diff_secs = cyc_act_start;
		if (neg) {
			diff_secs *= -1;
			put_bucket = (int)diff_secs;
			if (put_bucket >= neg_num_buckets) {
				printf("error: no (neg)bucket[%d] to put value %.16f in\n",put_bucket,diff_secs);
				printf("%llu  %llu\n", rec_jitter[i].theo_start, rec_jitter[i].actual_start);
				printf("%.16f %.16f\n", cvt_cycles_microsecs(rec_jitter[i].theo_start), cvt_cycles_microsecs(rec_jitter[i].actual_start) );
			} else {
				bucket[put_bucket]++;
			}	
		} else {
			put_bucket = (int)diff_secs;
			if (put_bucket >= pos_num_buckets) {
				printf("error: no (pos)bucket[%d] to put value %.16f in\n",put_bucket,diff_secs);
				printf("%llu  %llu\n", rec_jitter[i].theo_start, rec_jitter[i].actual_start);
				printf("%.16f %.16f\n", cvt_cycles_microsecs(rec_jitter[i].theo_start), cvt_cycles_microsecs(rec_jitter[i].actual_start) );
			} else {	
				bucket[put_bucket+neg_num_buckets]++;
			}
		}

	}

	fp=fopen(OUTPUT_FILE, "w+");
	if (!fp) {
		printf("error opening %s\n", OUTPUT_FILE);
		return;
	}

	for (i=0; i<total_buckets; ++i) {
		fprintf(fp, "%d %d\n", i-neg_num_buckets, bucket[i]);
	}

	fclose(fp);
}

/**
 * Here we will initialize all the global variables we will need.
 */
int initialize_globals() {
	int secs_to_wait_for_hz = HZ_ACCURACY;
	int rtn_val = 0;
	int i;

	// here we want to get the speed of the cpu
	printf("obtaining cpu hz\n");
	cpu_hz = get_cpu_hz(secs_to_wait_for_hz);

	secs_per_cycle = 1.0L/cpu_hz;

	return rtn_val;	
}


int main(int argc, char **argv) {
	int ret;
	pthread_t tid;
	pthread_attr_t attr;

	if (initialize_globals()) {
		printf("ERROR: initializing globals\n");
	}

	printf("CPU_HZ: %.16f\n", cpu_hz);
	printf("secs/cycle: %.16f\n", secs_per_cycle);
	printf("number of trials: %d\n", NUM_REC_JITTER);

	lock_pages();

	init_pthread_attr(&attr);
	print_pthread_attr_priority(&attr);
	CHECK( pthread_create(&tid, &attr, test_period, NULL) );

	ret = pthread_join(tid,(void **)NULL);

	output_results();

	return 0;
}