#ifndef HIGH_RES_TIME_FUNCTIONS_H
#define HIGH_RES_TIME_FUNCTIONS_H

#include <ctime>
#include <sys/time.h>
#include <cmath>
/* Requires the librt and libm libraries */

static const long NSEC_PER_SEC = 1000000000L;

static inline bool timespec_greater(const struct timespec* t1, const struct timespec* t0){
  return ((t1->tv_sec > t0->tv_sec) || ((t1->tv_sec == t0->tv_sec) && (t1->tv_nsec > t0->tv_nsec)));
}

static inline bool timespec_greater(const struct timespec t1, const struct timespec t0){
  return ((t1.tv_sec > t0.tv_sec) || ((t1.tv_sec == t0.tv_sec) && (t1.tv_nsec > t0.tv_nsec)));
}

static inline bool timespec_less(const struct timespec* t1, const struct timespec* t0){
  return ((t1->tv_sec < t0->tv_sec) || ((t1->tv_sec == t0->tv_sec) && (t1->tv_nsec < t0->tv_nsec)));
}

static inline bool timespec_less(const struct timespec t1, const struct timespec t0){
  return ((t1.tv_sec < t0.tv_sec) || ((t1.tv_sec == t0.tv_sec) && (t1.tv_nsec < t0.tv_nsec)));
}

static inline bool timespec_equal(const struct timespec* t1, const struct timespec* t0){
  return ((t1->tv_sec == t0->tv_sec) && (t1->tv_nsec == t0->tv_nsec));
}

static inline bool timespec_equal(const struct timespec t1, const struct timespec t0){
  return ((t1.tv_sec == t0.tv_sec) && (t1.tv_nsec == t0.tv_nsec));
}

static inline void timespec_reset(struct timespec* ret){
  ret->tv_sec = 0;
  ret->tv_nsec = 0;
}

static inline void set_normalized_timespec(struct timespec *ts, time_t sec, long nsec){
  while (nsec > NSEC_PER_SEC){
    nsec -= NSEC_PER_SEC;
    ++sec;
  }
  while(nsec < 0){
    nsec += NSEC_PER_SEC;
    --sec;
  }
  ts->tv_sec = sec;
  ts->tv_nsec = nsec;
}

static inline struct timespec convert_to_timespec(const double timeValue){
  struct timespec ret;
  double seconds = 0;
  long nsec = static_cast<long>(modf(timeValue, &seconds) * static_cast<double>(NSEC_PER_SEC));
  time_t sec = static_cast<time_t>(seconds);

  set_normalized_timespec(&ret, sec, nsec);

  return ret;
}

static inline double convert_from_timespec(const timespec actual){
  return (static_cast<double>(actual.tv_sec) + (static_cast<double>(actual.tv_nsec) / static_cast<double>(NSEC_PER_SEC)));
}

static inline void timespec_add(struct timespec *ret, const struct timespec* t1, const struct timespec* t0){
  time_t sec = t1->tv_sec + t0->tv_sec;
  long nsec = t1->tv_nsec + t0->tv_nsec;

  set_normalized_timespec(ret, sec, nsec);
}

static inline void timespec_add(struct timespec *ret, const struct timespec t1, const struct timespec t0){
  return timespec_add(ret, &t1, &t0);
}

static inline struct timespec timespec_add(const struct timespec t1, const struct timespec t0){
  struct timespec ret;
  timespec_add(&ret, &t1, &t0);
  return ret;
}

static inline struct timespec timespec_add(const struct timespec t1, const double time0){
  struct timespec ret;
  struct timespec t0;
  t0 = convert_to_timespec(time0);

  timespec_add(&ret, &t1, &t0);

  return ret;
}

static inline void timespec_subtract(struct timespec *ret, const struct timespec* t1, const struct timespec* t0){
  time_t sec = t1->tv_sec - t0->tv_sec;
  long nsec = t1->tv_nsec - t0->tv_nsec;

  set_normalized_timespec(ret, sec, nsec);
}

static inline void timespec_subtract(struct timespec *ret, const struct timespec t1, const struct timespec t0){
  return timespec_subtract(ret, &t1, &t0);
}

static inline struct timespec timespec_subtract(const struct timespec t1, const struct timespec t0){
  struct timespec ret;
  timespec_subtract(&ret, &t1, &t0);
  return ret;
}

static inline struct timespec timespec_subtract(const struct timespec t1, const double time0){
  struct timespec ret;
  struct timespec t0;
  t0 = convert_to_timespec(time0);

  timespec_subtract(&ret, &t1, &t0);

  return ret;
}

static inline double diff_timespec(struct timespec* ret, const struct timespec *t1, const struct timespec* t0){
  struct timespec actual;
  time_t sec = 0;
  long nsec = 0;

  if(timespec_greater(t1, t0)){
    sec = t1->tv_sec - t0->tv_sec;
    nsec = t1->tv_nsec - t0->tv_nsec;

    set_normalized_timespec(&actual, sec, nsec);
    
    if(ret != NULL){
      ret->tv_sec = actual.tv_sec;
      ret->tv_nsec = actual.tv_nsec;
    }

    return convert_from_timespec(actual);
  }
  else{
    sec = t0->tv_sec - t1->tv_sec;
    nsec = t0->tv_nsec - t1->tv_nsec;

    // Do nothing with the ret value as the ret value would have to store a negative, which it can't.

    set_normalized_timespec(&actual, sec, nsec);
    
    return (-convert_from_timespec(actual));
  }
}

static inline double diff_timespec(struct timespec* ret, const struct timespec t1, const struct timespec t0){
  return diff_timespec(ret, &t1, &t0);
}

static inline double diff_timespec(const struct timespec t1, const struct timespec t0){
  return diff_timespec(NULL, &t1, &t0);
}


static inline double diff_timespec(const struct timespec* t1, const struct timespec* t0){
  return diff_timespec(NULL, t1, t0);
}


static inline void get_highres_clock(struct timespec* ret){
  if(clock_gettime(CLOCK_REALTIME, ret) != 0){
    // Unable to get high resolution time - fail over to low resolution time
    timeval lowResTime;
    gettimeofday(&lowResTime, NULL);
    ret->tv_sec = lowResTime.tv_sec;
    ret->tv_nsec = lowResTime.tv_usec*1000;
  }
}

static inline struct timespec get_highres_clock(){
  struct timespec ret;
  get_highres_clock(&ret);
  return ret;
}

static inline bool timespec_empty(const struct timespec* ret){
  return ( (ret->tv_sec == 0 ) &&  (ret->tv_nsec == 0) );
}

static inline bool timespec_empty(const struct timespec ret){
  return timespec_empty(&ret);
}

#endif /* HIGH_RES_TIME_FUNCTIONS_H */
