#include "thread_publisher.h"

template <typename T>
ThreadPublisher<T>::ThreadPublisher() {
  shutdown_requested_ = false;
}

template <typename T>
ThreadPublisher<T>::~ThreadPublisher() {
}

template <typename T>
void ThreadPublisher<T>::Register(
    std::function<void(const std::shared_ptr<const T>&)> cb) {
  callbacks_.push_back(cb);
}

template <typename T>
void ThreadPublisher<T>::Start() {
  publish_thread_ = std::thread(std::bind(&ThreadPublisher::Process, this));
}

template <typename T>
void ThreadPublisher<T>::Publish(const std::shared_ptr<const T> msg) {
  std::unique_lock<std::mutex> locker(msg_mutex_);
  msgs_.push(msg);
  locker.unlock();
  msg_cond_.notify_one();
}

template <typename T>
void ThreadPublisher<T>::Process() {
  while (!shutdown_requested_) {
    std::shared_ptr<const T> msg;
    std::unique_lock<std::mutex> locker(msg_mutex_);
    while (msgs_.empty()) {
      if (shutdown_requested_) {
        locker.unlock();
        break;
      } else {
        msg_cond_.wait(locker);
      }
    }

    if (shutdown_requested_) {
      break;
    } else {
      msg = msgs_.back();
      while (!msgs_.empty()) {
        msgs_.pop();
      }
    }
    locker.unlock();

    for (auto callback : callbacks_) {
      callback(msg);
    }
  }
}

template <typename T>
void ThreadPublisher<T>::ShutDown() {
  shutdown_requested_ = true;
  msg_cond_.notify_one();
  if (publish_thread_.joinable()) {
    publish_thread_.join();
  }
}
