SimTime.cpp

Go to the documentation of this file.

#include "SimTime.h"
 
#ifdef MADARA_FEATURE_SIMTIME
 
#include <chrono>
#include <cmath>
 
namespace madara
{
namespace utility
{
const double minrate = 0.0000000001;
 
std::mutex SimTime::mutex_{};
 
sim_time_callback_fn SimTime::callback_ = nullptr;
 
uint64_t SimTime::last_realtime_ = SimTime::realtime();
uint64_t SimTime::last_simtime_ = -1;
double SimTime::last_rate_ = 1.0;
 
uint64_t SimTime::realtime()
{
  namespace sc = std::chrono;
  auto now = sc::steady_clock::now();
  auto dur = now.time_since_epoch();
  return sc::duration_cast<sc::nanoseconds>(dur).count();
}
 
uint64_t SimTime::realtime(uint64_t simtime)
{
  uint64_t prt;
  uint64_t pst;
  double pr;
  sim_time_callback_fn callback;
 
  {
    std::lock_guard<std::mutex> guard{mutex_};
    callback = callback_;
    if (callback)
    {
      uint64_t st;
      double r;
 
      callback_(&st, &r);
 
      prt = last_realtime_ = realtime();
      pst = last_simtime_ = st;
      pr = last_rate_ = r;
    }
    else
    {
      prt = last_realtime_;
      pst = last_simtime_;
      pr = last_rate_;
    }
  }
 
  if (pst == (uint64_t)-1)
  {
    return simtime;
  }
 
  if (pr == 0)
  {
    pr = 1;
  }
 
  return ((simtime - pst) / pr) + prt;
}
 
uint64_t SimTime::time()
{
  uint64_t prt;
  uint64_t pst;
  double pr;
  uint64_t st;
  double r;
  sim_time_callback_fn callback;
 
  int64_t now = realtime();
  {
    std::lock_guard<std::mutex> guard{mutex_};
    callback = callback_;
    if (callback)
    {
      callback_(&st, &r);
 
      last_realtime_ = now;
      last_simtime_ = st;
      last_rate_ = r;
    }
    else
    {
      prt = last_realtime_;
      pst = last_simtime_;
      pr = last_rate_;
    }
  }
 
  if (!callback)
  {
    if (pst == (uint64_t)-1)
    {
      return now;
    }
    if (pr == 0)
    {
      return pst;
    }
 
    int64_t offset = now - prt;
 
    if (pr < minrate)
    {
      pr = minrate;
    }
 
    double delta = offset * pr;
    st = pst + (int64_t)delta;
  }
 
  return st;
}
 
double SimTime::rate()
{
  double r;
  sim_time_callback_fn callback;
 
  {
    std::lock_guard<std::mutex> guard{mutex_};
    callback = callback_;
 
    if (callback)
    {
      callback(nullptr, &r);
    }
    else
    {
      r = last_rate_;
    }
  }
 
  return r;
}
 
uint64_t SimTime::duration(uint64_t sim_duration)
{
  double r = rate();
 
  if (r < minrate)
  {
    return -1;
  }
 
  return sim_duration / r;
}
 
uint64_t SimTime::future(uint64_t sim_offset)
{
  uint64_t now = realtime();
  uint64_t offset = duration(sim_offset);
 
  if (offset == (uint64_t)-1)
  {
    return -1;
  }
 
  return now + offset;
}
 
sim_time_callback_fn set_sim_time_callback(sim_time_callback_fn fn)
{
  std::lock_guard<std::mutex> guard{SimTime::mutex_};
  using std::swap;
  swap(fn, SimTime::callback_);
  return fn;
}
 
void sim_time_notify(uint64_t time, double rate)
{
  bool update_time = time != (uint64_t)-1;
  bool update_rate = !std::isnan(rate);
 
  if (!update_time && !update_rate)
  {
    return;
  }
 
  uint64_t now = SimTime::realtime();
  std::lock_guard<std::mutex> guard{SimTime::mutex_};
 
  if (update_time)
  {
    SimTime::last_realtime_ = now;
    SimTime::last_simtime_ = time;
  }
  else if (SimTime::last_simtime_ == (uint64_t)-1)
  {
    SimTime::last_realtime_ = now;
    SimTime::last_simtime_ = now;
  }
 
  if (update_rate)
  {
    SimTime::last_rate_ = rate;
  }
}
}
}
 
#endif