allwpilib/hal/include/HAL/cpp/priority_condition_variable.h

#pragma once

/* std::condition_variable provides the native_handle() method to return its
 * underlying pthread_cond_t*. WPILib uses this to interface with the FRC
 * network communication library. Since WPILib uses a custom mutex class and
 * not std::mutex, std::condition_variable_any must be used instead.
 * std::condition_variable_any doesn't expose its internal handle, so this
 * class provides the same interface and implementation in addition to a
 * native_handle() method.
 */

#include <condition_variable>
#include <memory>
#include "priority_mutex.h"

class priority_condition_variable {
  typedef pthread_cond_t* native_handle_type;
  typedef std::chrono::system_clock clock_t;

 public:
  priority_condition_variable() : m_mutex(std::make_shared<std::mutex>()) {}
  ~priority_condition_variable() = default;

  priority_condition_variable(const priority_condition_variable&) = delete;
  priority_condition_variable& operator=(const priority_condition_variable&) = delete;

  void notify_one() noexcept {
    std::lock_guard<std::mutex> lock(*m_mutex);
    m_cond.notify_one();
  }

  void notify_all() noexcept {
    std::lock_guard<std::mutex> lock(*m_mutex);
    m_cond.notify_all();
  }

  template<typename Lock>
  void wait(Lock& _lock) {
    std::shared_ptr<std::mutex> _mutex = m_mutex;
    std::unique_lock<std::mutex> my_lock(*_mutex);
    Unlock<Lock> unlock(_lock);

    // *mutex must be unlocked before re-locking _lock so move
    // ownership of *_mutex lock to an object with shorter lifetime.
    std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
    m_cond.wait(my_lock2);
  }

  template<typename Lock, typename Predicate>
  void wait(Lock& lock, Predicate p) {
    while (!p()) { wait(lock); }
  }

  template<typename Lock, typename Clock, typename Duration>
  std::cv_status wait_until(Lock& _lock,
        const std::chrono::time_point<Clock, Duration>& atime) {
    std::shared_ptr<std::mutex> _mutex = m_mutex;
    std::unique_lock<std::mutex> my_lock(*_mutex);
    Unlock<Lock> unlock(_lock);

    // *_mutex must be unlocked before re-locking _lock so move
    // ownership of *_mutex lock to an object with shorter lifetime.
    std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
    return m_cond.wait_until(my_lock2, atime);
  }

  template<typename Lock, typename Clock, typename Duration, typename Predicate>
  bool wait_until(Lock& lock,
      const std::chrono::time_point<Clock, Duration>& atime, Predicate p) {
    while (!p()) {
      if (wait_until(lock, atime) == std::cv_status::timeout) {
        return p();
      }
    }
    return true;
  }

  template<typename Lock, typename Rep, typename Period>
  std::cv_status wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime) {
    return wait_until(lock, clock_t::now() + rtime);
  }

  template<typename Lock, typename Rep, typename Period, typename Predicate>
  bool wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime,
      Predicate p) {
    return wait_until(lock, clock_t::now() + rtime, std::move(p));
  }

  native_handle_type native_handle() {
    return m_cond.native_handle();
  }

 private:
  std::condition_variable m_cond;
  std::shared_ptr<std::mutex> m_mutex;

  // scoped unlock - unlocks in ctor, re-locks in dtor
  template<typename Lock>
  struct Unlock {
    explicit Unlock(Lock& lk) : m_lock(lk) { lk.unlock(); }

    ~Unlock() noexcept(false) {
      if (std::uncaught_exception()) {
        try { m_lock.lock(); } catch(...) {}
      }
      else {
        m_lock.lock();
      }
    }

    Unlock(const Unlock&) = delete;
    Unlock& operator=(const Unlock&) = delete;

    Lock& m_lock;
  };
};