text: /*
   * Copyright (C) 2009, Willow Garage, Inc.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions are met:
   *   * Redistributions of source code must retain the above copyright notice,
   *     this list of conditions and the following disclaimer.
   *   * Redistributions in binary form must reproduce the above copyright
   *     notice, this list of conditions and the following disclaimer in the
   *     documentation and/or other materials provided with the distribution.
   *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
   *     contributors may be used to endorse or promote products derived from
   *     this software without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
   * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   * POSSIBILITY OF SUCH DAMAGE.
   */
  
  #ifndef ROSCPP_TIMER_MANAGER_H
  #define ROSCPP_TIMER_MANAGER_H
  
  // check if we might need to include our own backported version boost::condition_variable
  // in order to use CLOCK_MONOTONIC for the SteadyTimer
  // the include order here is important!
  #ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
  #include <boost/version.hpp>
  #if BOOST_VERSION < 106100
  // use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377
  #include "boost_161_condition_variable.h"
  #else // Boost version is 1.61 or greater and has the steady clock fixes
  #include <boost/thread/condition_variable.hpp>
  #endif
  #else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
  #include <boost/thread/condition_variable.hpp>
  #endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
  
  #include "ros/forwards.h"
  #include "ros/time.h"
  #include "ros/file_log.h"
  
  #include <boost/thread/thread.hpp>
  #include <boost/thread/mutex.hpp>
  #include <boost/thread/recursive_mutex.hpp>
  
  #include "ros/assert.h"
  #include "ros/callback_queue_interface.h"
  
  #include <vector>
  #include <list>
  
  namespace ros
  {
  
  template<class T, class D, class E>
  class TimerManager
  {
  private:
    struct TimerInfo
    {
      int32_t handle;
      D period;
  
      boost::function<void(const E&)> callback;
      CallbackQueueInterface* callback_queue;
  
      WallDuration last_cb_duration;
  
      T last_expected;
      T next_expected;
  
      T last_real;
  
      bool removed;
  
      VoidConstWPtr tracked_object;
      bool has_tracked_object;
  
      // TODO: atomicize
      boost::mutex waiting_mutex;
      uint32_t waiting_callbacks;
  
      bool oneshot;
  
      // debugging info
      uint32_t total_calls;
    };
    typedef boost::shared_ptr<TimerInfo> TimerInfoPtr;
    typedef boost::weak_ptr<TimerInfo> TimerInfoWPtr;
    typedef std::vector<TimerInfoPtr> V_TimerInfo;
  
    typedef std::list<int32_t> L_int32;
  
  public:
    TimerManager();
    ~TimerManager();
  
    int32_t add(const D& period, const boost::function<void(const E&)>& callback, CallbackQueueInterface* callback_queue, const VoidConstPtr& tracked_object, bool oneshot);
    void remove(int32_t handle);
  
    bool hasPending(int32_t handle);
    void setPeriod(int32_t handle, const D& period, bool reset=true);
  
    static TimerManager& global()
    {
      static TimerManager<T, D, E> global;
      return global;
    }
  
  private:
    void threadFunc();
  
    bool waitingCompare(int32_t lhs, int32_t rhs);
    TimerInfoPtr findTimer(int32_t handle);
    void schedule(const TimerInfoPtr& info);
    void updateNext(const TimerInfoPtr& info, const T& current_time);
  
    V_TimerInfo timers_;
    boost::mutex timers_mutex_;
    boost::condition_variable timers_cond_;
    volatile bool new_timer_;
  
    boost::mutex waiting_mutex_;
    L_int32 waiting_;
  
    uint32_t id_counter_;
    boost::mutex id_mutex_;
  
    bool thread_started_;
  
    boost::thread thread_;
  
    bool quit_;
  
    class TimerQueueCallback : public CallbackInterface
    {
    public:
      TimerQueueCallback(TimerManager<T, D, E>* parent, const TimerInfoPtr& info, T last_expected, T last_real, T current_expected)
      : parent_(parent)
      , info_(info)
      , last_expected_(last_expected)
      , last_real_(last_real)
      , current_expected_(current_expected)
      , called_(false)
      {
        boost::mutex::scoped_lock lock(info->waiting_mutex);
        ++info->waiting_callbacks;
      }
  
      ~TimerQueueCallback()
      {
        TimerInfoPtr info = info_.lock();
        if (info)
        {
          boost::mutex::scoped_lock lock(info->waiting_mutex);
          --info->waiting_callbacks;
        }
      }
  
      CallResult call()
      {
        TimerInfoPtr info = info_.lock();
        if (!info)
        {
          return Invalid;
        }
  
        {
          ++info->total_calls;
          called_ = true;
  
          VoidConstPtr tracked;
          if (info->has_tracked_object)
          {
            tracked = info->tracked_object.lock();
            if (!tracked)
            {
              return Invalid;
            }
          }
  
          E event;
          event.last_expected = last_expected_;
          event.last_real = last_real_;
          event.current_expected = current_expected_;
          event.current_real = T::now();
          event.profile.last_duration = info->last_cb_duration;
  
          SteadyTime cb_start = SteadyTime::now();
          info->callback(event);
          SteadyTime cb_end = SteadyTime::now();
          info->last_cb_duration = cb_end - cb_start;
  
          info->last_real = event.current_real;
  
          parent_->schedule(info);
        }
  
        return Success;
      }
  
    private:
      TimerManager<T, D, E>* parent_;
      TimerInfoWPtr info_;
      T last_expected_;
      T last_real_;
      T current_expected_;
  
      bool called_;
    };
  };
  
  template<class T, class D, class E>
  TimerManager<T, D, E>::TimerManager() :
    new_timer_(false), id_counter_(0), thread_started_(false), quit_(false)
  {
  
  }
  
  template<class T, class D, class E>
  TimerManager<T, D, E>::~TimerManager()
  {
    quit_ = true;
    {
      boost::mutex::scoped_lock lock(timers_mutex_);
      timers_cond_.notify_all();
    }
    if (thread_started_)
    {
      thread_.join();
    }
  }
  
  template<class T, class D, class E>
  bool TimerManager<T, D, E>::waitingCompare(int32_t lhs, int32_t rhs)
  {
    TimerInfoPtr infol = findTimer(lhs);
    TimerInfoPtr infor = findTimer(rhs);
    if (!infol || !infor)
    {
      return infol < infor;
    }
  
    return infol->next_expected < infor->next_expected;
  }
  
  template<class T, class D, class E>
  typename TimerManager<T, D, E>::TimerInfoPtr TimerManager<T, D, E>::findTimer(int32_t handle)
  {
    typename V_TimerInfo::iterator it = timers_.begin();
    typename V_TimerInfo::iterator end = timers_.end();
    for (; it != end; ++it)
    {
      if ((*it)->handle == handle)
      {
        return *it;
      }
    }
  
    return TimerInfoPtr();
  }
  
  template<class T, class D, class E>
  bool TimerManager<T, D, E>::hasPending(int32_t handle)
  {
    boost::mutex::scoped_lock lock(timers_mutex_);
    TimerInfoPtr info = findTimer(handle);
  
    if (!info)
    {
      return false;
    }
  
    if (info->has_tracked_object)
    {
      VoidConstPtr tracked = info->tracked_object.lock();
      if (!tracked)
      {
        return false;
      }
    }
  
    boost::mutex::scoped_lock lock2(info->waiting_mutex);
    return info->next_expected <= T::now() || info->waiting_callbacks != 0;
  }
  
  template<class T, class D, class E>
  int32_t TimerManager<T, D, E>::add(const D& period, const boost::function<void(const E&)>& callback, CallbackQueueInterface* callback_queue,
                                     const VoidConstPtr& tracked_object, bool oneshot)
  {
    TimerInfoPtr info(boost::make_shared<TimerInfo>());
    info->period = period;
    info->callback = callback;
    info->callback_queue = callback_queue;
    info->last_expected = T::now();
    info->next_expected = info->last_expected + period;
    info->removed = false;
    info->has_tracked_object = false;
    info->waiting_callbacks = 0;
    info->total_calls = 0;
    info->oneshot = oneshot;
    if (tracked_object)
    {
      info->tracked_object = tracked_object;
      info->has_tracked_object = true;
    }
  
    {
      boost::mutex::scoped_lock lock(id_mutex_);
      info->handle = id_counter_++;
    }
  
    {
      boost::mutex::scoped_lock lock(timers_mutex_);
      timers_.push_back(info);
  
      if (!thread_started_)
      {
        thread_ = boost::thread(boost::bind(&TimerManager::threadFunc, this));
        thread_started_ = true;
      }
  
      {
        boost::mutex::scoped_lock lock(waiting_mutex_);
        waiting_.push_back(info->handle);
        waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
      }
  
      new_timer_ = true;
      timers_cond_.notify_all();
    }
  
    return info->handle;
  }
  
  template<class T, class D, class E>
  void TimerManager<T, D, E>::remove(int32_t handle)
  {
    CallbackQueueInterface* callback_queue = 0;
    uint64_t remove_id = 0;
  
    {
      boost::mutex::scoped_lock lock(timers_mutex_);
  
      typename V_TimerInfo::iterator it = timers_.begin();
      typename V_TimerInfo::iterator end = timers_.end();
      for (; it != end; ++it)
      {
        const TimerInfoPtr& info = *it;
        if (info->handle == handle)
        {
          info->removed = true;
          callback_queue = info->callback_queue;
          remove_id = (uint64_t)info.get();
          timers_.erase(it);
          break;
        }
      }
  
      {
        boost::mutex::scoped_lock lock2(waiting_mutex_);
        // Remove from the waiting list if it's in it
        L_int32::iterator it = std::find(waiting_.begin(), waiting_.end(), handle);
        if (it != waiting_.end())
        {
          waiting_.erase(it);
        }
      }
    }
  
    if (callback_queue)
    {
      callback_queue->removeByID(remove_id);
    }
  }
  
  template<class T, class D, class E>
  void TimerManager<T, D, E>::schedule(const TimerInfoPtr& info)
  {
    boost::mutex::scoped_lock lock(timers_mutex_);
  
    if (info->removed)
    {
      return;
    }
  
    updateNext(info, T::now());
    {
      boost::mutex::scoped_lock lock(waiting_mutex_);
  
      waiting_.push_back(info->handle);
      // waitingCompare requires a lock on the timers_mutex_
      waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
    }
  
    new_timer_ = true;
    timers_cond_.notify_one();
  }
  
  template<class T, class D, class E>
  void TimerManager<T, D, E>::updateNext(const TimerInfoPtr& info, const T& current_time)
  {
    if (info->oneshot)
    {
      info->next_expected = T(INT_MAX, 999999999);
    }
    else
    {
      // Protect against someone having called setPeriod()
      // If the next expected time is already past the current time
      // don't update it
      if (info->next_expected <= current_time)
      {
        info->last_expected = info->next_expected;
        info->next_expected += info->period;
      }
  
      // detect time jumping forward, as well as callbacks that are too slow
      if (info->next_expected + info->period < current_time)
      {
        ROS_DEBUG("Time jumped forward by [%f] for timer of period [%f], resetting timer (current=%f, next_expected=%f)", (current_time - info->next_expected).toSec(), info->period.toSec(), current_time.toSec(), info->next_expected.toSec());
        info->next_expected = current_time;
      }
    }
  }
  
  template<class T, class D, class E>
  void TimerManager<T, D, E>::setPeriod(int32_t handle, const D& period, bool reset)
  {
    boost::mutex::scoped_lock lock(timers_mutex_);
    TimerInfoPtr info = findTimer(handle);
  
    if (!info)
    {
      return;
    }
  
    {
      boost::mutex::scoped_lock lock(waiting_mutex_);
    
      if(reset)
      {
        info->next_expected = T::now() + period;
      }
      
      // else if some time has elapsed since last cb (called outside of cb)
      else if( (T::now() - info->last_real) < info->period)
      {
        // if elapsed time is greater than the new period
        // do the callback now
        if( (T::now() - info->last_real) > period)
        {
          info->next_expected = T::now();
        }
     
        // else, account for elapsed time by using last_real+period
        else
        {
          info->next_expected = info->last_real + period;
        }
      }
      
      // Else if called in a callback, last_real has not been updated yet => (now - last_real) > period
      // In this case, let next_expected be updated only in updateNext
      
      info->period = period;
      waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
    }
  
    new_timer_ = true;
    timers_cond_.notify_one();
  }
  
  template<class T, class D, class E>
  void TimerManager<T, D, E>::threadFunc()
  {
    T current;
    while (!quit_)
    {
      T sleep_end;
  
      boost::mutex::scoped_lock lock(timers_mutex_);
  
      // detect time jumping backwards
      if (T::now() < current)
      {
        ROSCPP_LOG_DEBUG("Time jumped backward, resetting timers");
  
        current = T::now();
  
        typename V_TimerInfo::iterator it = timers_.begin();
        typename V_TimerInfo::iterator end = timers_.end();
        for (; it != end; ++it)
        {
          const TimerInfoPtr& info = *it;
  
          // Timer may have been added after the time jump, so also check if time has jumped past its last call time
          if (current < info->last_expected)
          {
            info->last_expected = current;
            info->next_expected = current + info->period;
          }
        }
      }
  
      current = T::now();
  
      {
        boost::mutex::scoped_lock waitlock(waiting_mutex_);
  
        if (waiting_.empty())
        {
          sleep_end = current + D(0.1);
        }
        else
        {
          TimerInfoPtr info = findTimer(waiting_.front());
  
          while (!waiting_.empty() && info && info->next_expected <= current)
          {
            current = T::now();
  
            //ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
            CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected));
            info->callback_queue->addCallback(cb, (uint64_t)info.get());
  
            waiting_.pop_front();
  
            if (waiting_.empty())
            {
              break;
            }
  
            info = findTimer(waiting_.front());
          }
  
          if (info)
          {
            sleep_end = info->next_expected;
          }
        }
      }
  
      while (!new_timer_ && T::now() < sleep_end && !quit_)
      {
        // detect backwards jumps in time
  
        if (T::now() < current)
        {
          ROSCPP_LOG_DEBUG("Time jumped backwards, breaking out of sleep");
          break;
        }
  
        current = T::now();
  
        if (current >= sleep_end)
        {
          break;
        }
  
        // If we're on simulation time we need to check now() against sleep_end more often than on system time,
        // since simulation time may be running faster than real time.
        if (!T::isSystemTime())
        {
          timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(1));
        }
        else
        {
          // On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
          // signal the condition variable
          int64_t remaining_time = std::max<int64_t>((sleep_end - current).toSec() * 1000.0f, 1);
          timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time));
        }
      }
  
      new_timer_ = false;
    }
  }
  
  }
  
  #endif
  