//
// Copyright (c) 2015-2019 CNRS INRIA
//

#ifndef __pinocchio_utils_timer_hpp__
#define __pinocchio_utils_timer_hpp__

#ifdef WIN32
  #include <Windows.h>
  #include <stdint.h> // portable: uint64_t   MSVC: __int64

int gettimeofday(struct timeval * tp, struct timezone * tzp)
{
  // Note: some broken versions only have 8 trailing zero's, the correct epoch has 9 trailing zero's
  // This magic number is the number of 100 nanosecond intervals since January 1, 1601 (UTC)
  // until 00:00:00 January 1, 1970
  static const uint64_t EPOCH = ((uint64_t)116444736000000000ULL);

  SYSTEMTIME system_time;
  FILETIME file_time;
  uint64_t time;

  GetSystemTime(&system_time);
  SystemTimeToFileTime(&system_time, &file_time);
  time = ((uint64_t)file_time.dwLowDateTime);
  time += ((uint64_t)file_time.dwHighDateTime) << 32;

  tp->tv_sec = (long)((time - EPOCH) / 10000000L);
  tp->tv_usec = (long)(system_time.wMilliseconds * 1000);
  return 0;
}
#else
  #include <sys/time.h>
#endif
#include <iostream>
#include <stack>

#define SMOOTH(s) for (size_t _smooth = 0; _smooth < s; ++_smooth)

/* Return the time spent in secs. */
inline double operator-(const struct timeval & t1, const struct timeval & t0)
{
  /* TODO: double check the double conversion from long (on 64x). */
  return double(t1.tv_sec - t0.tv_sec) + 1e-6 * double(t1.tv_usec - t0.tv_usec);
}

struct PinocchioTicToc
{
  enum Unit
  {
    S = 1,
    MS = 1000,
    US = 1000000,
    NS = 1000000000
  };
  Unit DEFAULT_UNIT;

  static std::string unitName(Unit u)
  {
    switch (u)
    {
    case S:
      return "s";
    case MS:
      return "ms";
    case US:
      return "us";
    case NS:
      return "ns";
    }
    return "";
  }

  std::stack<struct timeval> stack;
  mutable struct timeval t0;

  PinocchioTicToc(Unit def = MS)
  : DEFAULT_UNIT(def)
  {
  }

  inline void tic()
  {
    stack.push(t0);
    gettimeofday(&(stack.top()), NULL);
  }

  inline double toc()
  {
    return toc(DEFAULT_UNIT);
  };

  inline double toc(const Unit factor)
  {
    gettimeofday(&t0, NULL);
    double dt = (t0 - stack.top()) * (double)factor;
    stack.pop();
    return dt;
  }

  inline void toc(std::ostream & os, double SMOOTH = 1)
  {
    os << toc(DEFAULT_UNIT) / SMOOTH << " " << unitName(DEFAULT_UNIT) << std::endl;
  }
};

#endif // ifndef __pinocchio_utils_timer_hpp__
