#define QOS_DEBUG_LEVEL QOS_LEVEL_ERROR
#include <linux/aquosa/qos_debug.h>

#include <aquosa/qres_lib.h>

#include <assert.h>
#include <unistd.h>
#include <stdio.h>
#include <signal.h>
#include <sys/time.h>
#include <stdlib.h>
#include <time.h>

#include <errno.h>
#include <string.h>
#include <sched.h>

#include "queue.h"

queue_t q;

unsigned long counted_in_period = 0;
long first_time = 0;
long last_time = 0;
long sum_delta_time = 0;
long enqueued_sum_dt = 0;
long enqueued_max_dt = 0;
long job_cnt = 0;

// Do not add parenthesis, please.
#define FRAGM 10

int job(void *);

int use_qos = 0;
int use_rt = 0;
int use_load = 0;
int ratio = 25;

/************************************************************/

int enqueued;		/* Number of enqueued jobs */
int fired;	/* Number of fired alarms */

long period_nsec = 40000000; /* Is approximated according to timer resolution */

struct timeval ref_tv;
long unsigned deadline_nsec;
struct timeval tv;

void alarm_handler(int i) {
  gettimeofday(&tv, NULL);
  fired++;
  qos_log_debug("Handler called at: %lu, enqueued=%d\n", (tv.tv_sec - ref_tv.tv_sec) * 1000000 + tv.tv_usec - ref_tv.tv_usec - deadline_nsec, enqueued);
  enqueued++;
  deadline_nsec += period_nsec;

  job(NULL);
}

/** Signal mask containing the single SIGALRM signal	*/
static sigset_t sig_set;

static int qmgr_periodic_started = 0;
static void (*qmgr_old_handler)(int);

/** @todo Replace chk_exit with proper error report to caller */
int start_periodic_alarm() {
  struct itimerval itv;
  struct timespec ts;

  clock_getres(CLOCK_REALTIME, &ts);
  printf("Requested period: %lu nsec\n", period_nsec);
  printf("Timer resolution: %lu sec and %lu nsec\n", ts.tv_sec, ts.tv_nsec);
  unsigned long timer_res = ts.tv_sec * 1000000000ul + ts.tv_nsec;
  unsigned long new_period_nsec = period_nsec / timer_res * timer_res;
  while (new_period_nsec < period_nsec)
    new_period_nsec += timer_res;
  period_nsec = new_period_nsec;
  printf("Real period: %lu nsec\n", period_nsec);

  gettimeofday(&ref_tv, NULL);
  deadline_nsec = period_nsec;

  enqueued = 0;
  qmgr_periodic_started = 1;

  /* Set handler for alarm */
  qmgr_old_handler = signal(SIGALRM, &alarm_handler);
  if (qmgr_old_handler == SIG_ERR) {
    qos_log_err("Could not set SIGALRM handler");
    return QOS_E_GENERIC;
  } else if (qmgr_old_handler != SIG_DFL) {
    qos_log_err("Handler already set for SIGALRM");
    return QOS_E_GENERIC;
  }

  /* Schedule a periodic SIGALRM delivery */
  itv.it_interval.tv_sec = period_nsec / 1000000000;
  itv.it_interval.tv_usec = (period_nsec / 1000) % 1000000;
  itv.it_value.tv_sec = period_nsec / 1000000000;
  itv.it_value.tv_usec = (period_nsec / 1000) % 1000000;

/*   if (setitimer(ITIMER_REAL, &itv, NULL) != 0) { */
/*     signal(SIGALRM, qmgr_old_handler); */
/*     qos_log_err("setitimer() failed"); */
/*     return QOS_E_GENERIC; */
/*   } */

  struct itimerspec its = {
    .it_interval = {
      .tv_sec = period_nsec / 1000000000,
      .tv_nsec = period_nsec % 1000000000
    },
    .it_value = {
      .tv_sec = period_nsec / 1000000000,
      .tv_nsec = period_nsec % 1000000000
    }
  };
  struct sigevent se = {
    .sigev_notify = SIGEV_SIGNAL,
    .sigev_signo = SIGALRM
  };
  timer_t timer_id;
  if (timer_create(CLOCK_REALTIME, &se, &timer_id) < 0) {
    signal(SIGALRM, qmgr_old_handler);
    qos_log_err("timer_create() failed: %s", strerror(errno));
    return QOS_E_GENERIC;
  }
  if (timer_settime(timer_id, 0, &its, NULL) < 0) {
    signal(SIGALRM, qmgr_old_handler);
    qos_log_err("timer_settime() failed: %s", strerror(errno));
    return QOS_E_GENERIC;
  }

  /* Create a mask with only the SIGALRM	*/
  sigemptyset(&sig_set);
  sigaddset(&sig_set, SIGALRM);
  /* Be sure SIGALRM in unblocked		*/
  sigprocmask(SIG_UNBLOCK, &sig_set, NULL);

  return QOS_OK;
}

/** @todo Replace chk_exit with proper error report to caller */
int qmgr_wait_periodic_alarm(void) {
  sigset_t old_set;

  // TODO
/*   if (qmgr_fd == -1 || ! qmgr_periodic_started) */
/*     return QOS_E_INCONSISTENT_STATE; */

  /* Block SIGALRM signal and get old mask	*/
  qos_chk_exit(sigprocmask(SIG_BLOCK, &sig_set, &old_set) == 0);
  /* Repeat check (if interrupted by another signal)	*/
  while (enqueued == 0) {
    /* There are no enqueued jobs: wait for next SIGALRM	*/
    sigsuspend(&old_set);
    /* Here we know the signal handler already completed	*/
  }
  enqueued--;
  /* Unblock SIGALRM signal			*/
  qos_chk_exit(sigprocmask(SIG_UNBLOCK, &sig_set, NULL) == 0);

  return QOS_OK;
}

int qmgr_stop_periodic_alarm(void) {
  struct itimerval itv;
  int rv = QOS_OK;

  // TODO:
/*   if (qmgr_fd == -1 || ! qmgr_periodic_started) */
/*     return QOS_E_INCONSISTENT_STATE; */

  itv.it_interval.tv_sec = 0;
  itv.it_interval.tv_usec = 0;
  itv.it_value.tv_sec = 0;
  itv.it_value.tv_usec = 0;
  if (setitimer(ITIMER_REAL, &itv, NULL) != 0) {
    qos_log_err("setitimer() failed");
    rv = QOS_E_GENERIC;
  }
  signal(SIGALRM, qmgr_old_handler);

  qmgr_periodic_started = 0;

  return rv;
}

/************************************************************/


/** Actively increments a counter and gets time until 1 (period_nsec / 1000)
 * elapsed. Returns the counter value. */
long count_and_wait() {
  unsigned long counted_sheeps = 0;
  unsigned long elapsed_usecs;
  struct timeval tv1, tv2;
  unsigned long duration_usec = period_nsec / 1000;
  qos_log_debug("Counting sheeps for %lu usecs ...", duration_usec);
  gettimeofday(&tv1, NULL);
  do {
    gettimeofday(&tv2, NULL);
    elapsed_usecs = tv2.tv_usec - tv1.tv_usec + (tv2.tv_sec - tv1.tv_sec) * 1000000;
    counted_sheeps++;
  } while (elapsed_usecs < duration_usec);
  qos_log_debug("Counted %lu sheeps", counted_sheeps);
  return counted_sheeps;
}

/** Loop identical to the one in count_and_wait, but runs until
 *  the counter goes to the supplied parameter. Returns the elapsed
 *  usecs during the count */
unsigned long count(unsigned long sheeps) {
  unsigned long counted_sheeps = 0;
  unsigned long elapsed_usecs;
  struct timeval tv1, tv2;

  qos_log_debug("Counting %lu sheeps ...", sheeps);
  gettimeofday(&tv1, NULL);
  do {
    gettimeofday(&tv2, NULL);
    elapsed_usecs = tv2.tv_usec - tv1.tv_usec + (tv2.tv_sec - tv1.tv_sec) * 1000000;
    counted_sheeps++;
  } while (counted_sheeps < sheeps);
  qos_log_debug("Counted %lu sheeps in %lu usecs", sheeps, elapsed_usecs);
  return elapsed_usecs;
}

/***********************************/

typedef struct {
  struct timespec deadline_ts;	/**< Next job expiration	*/
  struct timespec period_ts;	/**< Application period		*/
  int job_id;			/**< Job id			*/
} qmgr_period_spec_t;

/** Add two timespec values and normalize result */
static inline void timespec_add(struct timespec *dst_ts, struct timespec *src1_ts, struct timespec *src2_ts) {
  dst_ts->tv_sec = src1_ts->tv_sec + src2_ts->tv_sec;
  dst_ts->tv_nsec = src1_ts->tv_nsec + src2_ts->tv_nsec;
  long int delta_ns = dst_ts->tv_nsec - 1000000000lu;
  if (delta_ns >= 0) {
    dst_ts->tv_nsec = delta_ns;
    dst_ts->tv_sec++;
  }
}

/** Sub two timespec values and normalize result.
 *
 * @note This also works if dst_ts == src1 || dst_ts == src2 || src1 == src2
 */
static inline void timespec_sub(struct timespec *dst_ts, struct timespec *src1_ts, struct timespec *src2_ts) {
  dst_ts->tv_sec = src1_ts->tv_sec - src2_ts->tv_sec;
  dst_ts->tv_nsec = src1_ts->tv_nsec - src2_ts->tv_nsec;
  if (dst_ts->tv_nsec < 0) {
    dst_ts->tv_nsec = dst_ts->tv_nsec + 1000000000lu;
    dst_ts->tv_sec--;
  }
}

int qmgr_start_periodic(qmgr_period_spec_t *p_ps, struct timespec *p_ts) {
  int rv = QOS_E_GENERIC;

  p_ps->job_id = 1;

  /* Compute first job deadline */

  qos_log_debug("getting current time...\n");
  qos_chk_go(clock_gettime(CLOCK_REALTIME, &p_ps->deadline_ts) == 0,
    err, "clock_gettime() failed: %s", strerror(errno));

  qos_log_debug("read time: %09lu.%09lu\n", p_ps->deadline_ts.tv_sec, p_ps->deadline_ts.tv_nsec);

  p_ps->period_ts = *p_ts;
  timespec_add(&p_ps->deadline_ts, &p_ps->deadline_ts, &p_ps->period_ts);
  qos_log_debug("end of %dth period: %09lu.%09lu\n", p_ps->job_id,
    p_ps->deadline_ts.tv_sec, p_ps->deadline_ts.tv_nsec);

  return QOS_OK;

err:

  return rv;
}

int qmgr_wait_periodic(qmgr_period_spec_t *p_ps) {
  struct timespec now;
  struct timespec delta;

  p_ps->job_id++;

  if (clock_gettime(CLOCK_REALTIME, &now) < 0) {
    qos_log_err("clock_gettime() failed: %s", strerror(errno));
    return QOS_E_GENERIC;
  }

  timespec_sub(&delta, &p_ps->deadline_ts, &now);
  qos_log_debug("delta time: %09lu.%09lu", delta.tv_sec, delta.tv_nsec);
  if (delta.tv_sec > 0 || (delta.tv_sec == 0 && delta.tv_nsec > 0)) {
    qos_log_debug("Sleeping...");
    if (nanosleep(&delta, NULL) < 0) {
      qos_log_err("nanosleep() failed: %s", strerror(errno));
      return QOS_E_GENERIC;
    }
  } else
    qos_log_warn("We are late: not waiting...");

  timespec_add(&p_ps->deadline_ts, &p_ps->deadline_ts, &p_ps->period_ts);
  qos_log_debug("end of %dth period: %09lu.%09lu", p_ps->job_id,
    p_ps->deadline_ts.tv_sec, p_ps->deadline_ts.tv_nsec);

  return QOS_OK;
}

int job(void *ptr) {
  struct timeval tv;
  unsigned long curr_time;
  job_cnt++;
  if (job_cnt == 1) {
    gettimeofday(&tv, NULL);
    curr_time = (tv.tv_sec - ref_tv.tv_sec) * 1000000l + (tv.tv_usec - ref_tv.tv_usec);
    first_time = curr_time;
    printf("Time reference (%ld job): %lu\n", job_cnt, first_time);
    count(counted_in_period * ratio / 100);
    gettimeofday(&tv, NULL);
    curr_time = (tv.tv_sec - ref_tv.tv_sec) * 1000000l + (tv.tv_usec - ref_tv.tv_usec);
  } else {
    count(counted_in_period * ratio / 100);
    gettimeofday(&tv, NULL);
    curr_time = (tv.tv_sec - ref_tv.tv_sec) * 1000000l + (tv.tv_usec - ref_tv.tv_usec);
    unsigned long ideal_time = first_time + period_nsec/1000 * (job_cnt - 1);
    unsigned long delta_time = curr_time - ideal_time;
    //printf("%d: %lu - %lu = %ld (abs_dt=%ld)\n", job_cnt, curr_time, ideal_time, delta_time, curr_time - first_time);
    if (delta_time < 0)
      delta_time = -delta_time;
    int num_elems = queue_get_num_elements(&q);
    if (num_elems == 16) {
      long extracted_time = queue_extract(&q);
      enqueued_sum_dt -= extracted_time;
    }
    queue_insert(&q, delta_time);
    sum_delta_time += delta_time;
    enqueued_sum_dt += delta_time;

    queue_iterator_t qit = qit = queue_begin(&q);
    long max_dt = queue_it_next(&qit);
    while (queue_it_has_next(&qit)) {
      long dt = queue_it_next(&qit);
      if (dt > max_dt)
	max_dt = dt;
    }
    enqueued_max_dt = max_dt;

    if (num_elems > 0 && (job_cnt % (1000000000 / period_nsec)) == 0) {
      printf("avg dt=%ld, avg qdt=%ld, max qdt=%ld\n", sum_delta_time / job_cnt, enqueued_sum_dt / num_elems, enqueued_max_dt);
    }
  }
  last_time = curr_time;

  return 0;
}

void usage() {
  printf("Usage: rt-app [-P <period>] [--rt] [--qos] [--ratio <0-100>] [-d <num>]\n");
}

int main(int argc, char **argv) {
  argc--;  argv++;
  while (argc > 0) {
    if (strcmp(argv[0], "-P") == 0) {
      qos_chk_exit(argc > 0);
      argc--;  argv++;
      period_nsec = atol(argv[0]) * 1000ul;
    } else if (strcmp(argv[0], "--qos") == 0) {
      use_qos = 1;
    } else if (strcmp(argv[0], "--rt") == 0) {
      use_rt = 1;
    } else if (strcmp(argv[0], "--load") == 0) {
      use_load = 1;
    } else if (strcmp(argv[0], "-r") == 0 || strcmp(argv[0], "--ratio") == 0) {
      qos_chk_exit(argc > 0);
      argc--;  argv++;
      ratio = atoi(argv[0]);
    } else if (strcmp(argv[0], "--help") == 0 || strcmp(argv[0], "-h") == 0) {
      usage();
      exit(0);
    } else if (strcmp(argv[0], "-d") == 0) {
      qos_chk_exit(argc > 0);
      argc--;  argv++;
      counted_in_period = atol(argv[0]);
    } else {
      fprintf(stderr, "Unknown option: %s\n", argv[0]);
      usage();
      exit(-1);
    }
    argc--;  argv++;
  }

  struct sched_param sp = {
    .sched_priority = 20
  };
  if (use_rt) {
    printf("Setting RT priority...\n");
    if (sched_setscheduler(0, SCHED_RR, &sp) < 0) {
      fprintf(stderr, "Warning: couldn't set RT priority: %s\n", strerror(errno));
    }
  }

  if (counted_in_period == 0) {
    printf("Counting for 1 period...\n");
    counted_in_period = count_and_wait();
    printf("Counted in 1 period: %lu\n", counted_in_period);
  }

  qos_chk_exit(queue_init(&q, 16) == OK);

  if (use_qos) {
    if (use_rt) {
      sp.sched_priority = 0;
      printf("Unsetting RT priority...\n");
      if (sched_setscheduler(0, SCHED_OTHER, &sp) < 0) {
        fprintf(stderr, "Warning: couldn't set OTHER priority: %s\n", strerror(errno));
      }
    }
    qres_params_t params = {
      .Q_min = period_nsec / 1000 * ratio / 100 * 4 / 3 / FRAGM,
      .Q = period_nsec / 1000 * ratio / 100 * 4 / 3 / FRAGM,
      .P = period_nsec / 1000 / FRAGM,
      .flags = 0,
    };
    printf("Creating QRES Server with Q=%llu, P=%llu\n", params.Q, params.P);
    qos_chk_ok_exit(qres_init());
    qos_chk_ok_exit(qres_create_server(0, 0, &params));
  }

  if (use_load) {
	  printf("Computing and sleeping ...\n");
	  int i;
	  for (i = 0; i < 10; i++) {
	    count(counted_in_period);
	    unsigned long ns = (unsigned long) (1.0e9 / 25 * (rand() / (RAND_MAX + 1.0)));
	    printf("%d: %lu\n", i, ns);
	    struct timespec ts = {
	      .tv_sec = 0lu,
	      .tv_nsec = ns
	    };
	    nanosleep(&ts, NULL);
	  }
  }

  printf("Starting periodic task (using ratio: %d%%)...\n", ratio);
  qmgr_period_spec_t ps;
  struct timespec ts = {
    .tv_sec = period_nsec / 1000000000L,
    .tv_nsec = period_nsec % 1000000000L
  };
  qos_chk_ok_exit(qmgr_start_periodic(&ps, &ts));

  while (1) {
    job(NULL);
    qos_chk_ok_exit(qmgr_wait_periodic(&ps));
  }

  return 0;
}