#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <time.h>
#include <sched.h>

void timespec_add_ns(struct timespec *t, unsigned long long ns) {
  int tmp = ns / 1000000000;
  t->tv_sec += tmp;
  ns -= tmp * 1000000000;
  t->tv_nsec += ns;

  tmp = t->tv_nsec / 1000000000;
  t->tv_sec += tmp;
  t->tv_nsec -= tmp * 1000000000;
}

int main(int argc, char *argv[]) {
  struct timespec timer;
  struct timespec clock_tick;
  struct timespec ts1;
  struct sched_param param;
  int ret;

  /* Set the priority of the process */
  param.sched_priority = sched_get_priority_max(SCHED_FIFO);
  sched_setscheduler(0, SCHED_FIFO, &param);
  printf("Running with Priority: %d\n", param.sched_priority);

  /* Get the current clock resolution */
  clock_getres(CLOCK_REALTIME, &clock_tick);
  printf("CLOCK_REALTIME Resolution: %d s, %d ns\n\n", clock_tick.tv_sec, clock_tick.tv_nsec);

  /* Get the current time */
  clock_gettime(CLOCK_REALTIME, &timer);
  printf("timer1: %ds, %dns\n", timer.tv_sec, timer.tv_nsec);

  /* Add some interval to the current time */
  timespec_add_ns(&timer, 10000 * clock_tick.tv_nsec);
  printf("timer2: %ds, %dns\n", timer.tv_sec, timer.tv_nsec);

  /* Sleep until specified time */
  ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &timer, NULL);

  /* Get the current time */
  clock_gettime(CLOCK_REALTIME, &ts1);

  printf("ts1:    %ds, %dns\n", ts1.tv_sec, ts1.tv_nsec);
  printf("ret: %d\n", ret);

  return 0;
}