/*
 * Some of this code is from the program hourglass which is a program
 * to create synthetic real-time applications.
 * Copyright (c) 2001-2003 University of Utah and the Flux Group.  All
 * rights reserved.
*/

#include "utils.h"

struct timespec timespec_sub(struct timespec timespec_1,struct timespec timespec_2) {       
	struct timespec rtn_val;
	int xsec;
	int sign = 1;

	if ( timespec_2.tv_nsec > timespec_1.tv_nsec ) {
		xsec = (int)((timespec_2.tv_nsec - timespec_1.tv_nsec) / (1E9 + 1));
		timespec_2.tv_nsec -= (long int)(1E9 * xsec);
		timespec_2.tv_sec += xsec;
	}

	if ( (timespec_1.tv_nsec - timespec_2.tv_nsec) > 1E9 ) {
		xsec = (int)((timespec_1.tv_nsec - timespec_2.tv_nsec) / 1E9);
		timespec_2.tv_nsec += (long int)(1E9 * xsec);
		timespec_2.tv_sec -= xsec;
	}

	rtn_val.tv_sec = timespec_1.tv_sec - timespec_2.tv_sec;
	rtn_val.tv_nsec = timespec_1.tv_nsec - timespec_2.tv_nsec;                                                 

	if (timespec_1.tv_sec < timespec_2.tv_sec) {
		sign = -1;
	}

	rtn_val.tv_sec = rtn_val.tv_sec * sign;

	return rtn_val;
}

int clock_greater(struct timespec clock_1, struct timespec clock_2) {
	if (clock_1.tv_sec < clock_2.tv_sec) {
		return 0;
	} else if (clock_1.tv_sec == clock_2.tv_sec) {
		if (clock_1.tv_nsec < clock_2.tv_nsec) {
			return 0;
		}
	}

	return 1;
}

double get_secs_diff(struct timespec clock_1, struct timespec clock_2) {
	double rtn_val;
	struct timespec diff;

	diff = timespec_sub(clock_1, clock_2);
	
	rtn_val = diff.tv_sec;
	rtn_val += (double)diff.tv_nsec/(double)1E9;

	return rtn_val;		
}

double get_cpu_hz(int secs_to_wait) {
	int max_hz_jitter = (int)1E6;
	double rtn_hz=0;
	double last_hz = 0;
	double curr_hz;
	int i;
	struct timespec start_clock, finish_clock;
	cycles_t start_cycle, finish_cycle;
	struct timespec now, stop_time;
	double secs_diff;
	cycles_t diff_cycles;

	if (secs_to_wait < 1) {
		printf("ERROR: seconds to wait for cpu_hz < 1\n");
		exit (0);
	}

	set_priority(MAIN_PRI);

	for (i=0; i<10; ++i) {
		clock_gettime(CLOCK_REALTIME, &stop_time);
		stop_time.tv_sec += secs_to_wait;

		clock_gettime(CLOCK_REALTIME, &start_clock);
		start_cycle = get_cycles();

		set_priority(NORMAL);

		do {	
			clock_gettime(CLOCK_REALTIME, &now);
		} while ( !clock_greater(now, stop_time) );

		set_priority(MAIN_PRI);
		clock_gettime(CLOCK_REALTIME, &finish_clock);
		finish_cycle = get_cycles();

		secs_diff = get_secs_diff(start_clock, finish_clock);
		diff_cycles = finish_cycle - start_cycle;

		curr_hz = (double)diff_cycles/secs_diff;

		if ( DSIGNOF(last_hz-curr_hz)*(last_hz-curr_hz) < max_hz_jitter ) {
			rtn_hz = (last_hz + curr_hz)/(double)2;
			break;
		} else {
			last_hz = curr_hz;
		}

	}

	set_priority(NORMAL);
	if (rtn_hz != 0) {
		return rtn_hz;
	}
	printf("ERROR: could not find reliable HZ\n");
	exit (0);

}

void lock_pages (void) {		
	int res;
	    
	res = mlockall (MCL_CURRENT | MCL_FUTURE);
	if (res != 0) {
		perror ("mlockall");
		printf ("unable to lock pages into memory\n");
	}
}

static void make_sched_param (enum thrd_pri_t pri, 
			      int *policy,
			      struct sched_param *param,
			      int *niceval)
{
  int max_realtime_pri;

  max_realtime_pri = sched_get_priority_max (SCHED_FIFO);
  if (max_realtime_pri == -1) {
    perror ("sched_get_priority_max");
    exit (errno);
  }

  switch (pri) {

  case IDLE:
    param->sched_priority = 0;
    *policy = SCHED_OTHER;
    *niceval = 19;
    break;

  case LOW:
    param->sched_priority = 0;
    *policy = SCHED_OTHER;
    *niceval = 10;
    break;

  case NORMAL:
    param->sched_priority = 0;
    *policy = SCHED_OTHER;
    *niceval = 0;
    break;
    
  case HIGH:
    param->sched_priority = 0;
    *policy = SCHED_OTHER;
    *niceval = -10;
    break;
    
  case HIGHEST:
    param->sched_priority = 0;
    *policy = SCHED_OTHER;
    *niceval = -19;
    break;
    
  case RTLOW:
    param->sched_priority = max_realtime_pri / 4 - 2;
    *policy = SCHED_FIFO;
    break;
    
  case RTMED:
    param->sched_priority = max_realtime_pri / 2;
    *policy = SCHED_FIFO;
    break;
    
  case RTHIGH:
    param->sched_priority = max_realtime_pri - 2;
    *policy = SCHED_FIFO;
    break;
    
  case TIMER_PRI:
    param->sched_priority = max_realtime_pri - 1;
    *policy = SCHED_FIFO;
    break;

  case MAIN_PRI:
    param->sched_priority = max_realtime_pri;
    *policy = SCHED_FIFO;
    break;

  default:
    printf ("Hrm: unexpected arg to set_priority: %d\n", pri);
    exit (-1);
  }

}

void set_priority (enum thrd_pri_t pri)
{
  struct sched_param param;
  int policy, res, niceval;

  make_sched_param (pri, &policy, &param, &niceval);

  res = sched_setscheduler (0, policy, &param);
  if (res != 0) {
    perror ("sched_setscheduler");
    printf ("You probably need to be running as root.\n");
    exit (errno);
  }

  if (policy == SCHED_OTHER) {
    errno = 0;
    res = setpriority (PRIO_PROCESS, 0, niceval);
    if (errno != 0) {
      perror ("setpriority");
      exit (errno);
    }
  }
}