SampleClock.h

#pragma once
 
#include <fhsim/NFhSim.h>
#include <fhsim/PrintDuringExec.h>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <string>
 
class SampleClock
{
public:
   explicit SampleClock(const double period, const double firstSampleTime = 0.0)
       : m_period(period)
       , m_next(firstSampleTime)
   {
      if (!(std::isfinite(period) && period > 0.0))
         throw std::invalid_argument("SampleClock: the sample period must be finite and positive, it is " + std::to_string(period) + " s.");
   }
 
   static SampleClock Never() { return SampleClock(NeverTag {}); }
 
   void StartAt(const double tStart)
   {
      if (std::isfinite(m_period))
         m_next = tStart;
   }
 
   void SetLogger(PrintDuringExec* const print) { m_print = print; }
 
   bool Due(const double t)
   {
      if (t < m_next)
         return false;
 
      WarnOnceIfLate(t);
      m_next += m_period;
      return true;
   }
 
   double NextSampleTime() const { return m_next; }
 
   double Period() const { return m_period; }
 
   bool LatenessWarningGiven() const { return m_latenessWarningGiven; }
 
private:
   struct NeverTag
   {
   };
 
   explicit SampleClock(NeverTag)
       : m_period(std::numeric_limits<double>::infinity())
       , m_next(std::numeric_limits<double>::infinity())
   {
   }
 
   void WarnOnceIfLate(const double t)
   {
      const double lateness = t - m_next;
      if (lateness <= kLatenessFraction * m_period || m_latenessWarningGiven)
         return;
 
      m_latenessWarningGiven = true;
      if (m_print == nullptr)
         return;
 
      m_print->WriteLog("In SampleClock. A sample due at t = " + std::to_string(m_next) + " s was taken " + std::to_string(lateness) + " s late, which is more than " + std::to_string(kLatenessFraction) + " of the sample period of " + std::to_string(m_period) + " s. The integrator step is too coarse for this "
                                                                                                                                                                                                                                                                     "sample rate; set StepMax to " +
                              std::to_string(kLatenessFraction * m_period) + " s or less. This warning is given once.",
            fhsim::LogLevel_Warning);
   }
 
   static constexpr double kLatenessFraction = 0.25;
 
   double m_period; 
   double m_next;   
 
   PrintDuringExec* m_print                = nullptr; 
   bool             m_latenessWarningGiven = false;   
};