#ifndef _UT_ROBOT_PUBLISHER_H_
#define _UT_ROBOT_PUBLISHER_H_

#include <unitree/robot/channel/channel_publisher.hpp>
#include <atomic>
#include <thread>
#include <memory>

namespace unitree
{
namespace robot
{

template <typename MessageType>
class PublisherBase : public unitree::robot::ChannelPublisher<MessageType>
{
public:
  using MsgType = MessageType;
  using SharedPtr = std::shared_ptr<PublisherBase<MsgType>>;

  PublisherBase(std::string TOPIC_NAME)
  : unitree::robot::ChannelPublisher<MessageType>(TOPIC_NAME)
  {
    this->InitChannel();
  }
};

// For details: see https://github.com/ros-controls/realtime_tools
template <typename MessageType>
class RealTimePublisher
{
public:
  using MsgType = MessageType;
  using PublisherSharedPtr = typename unitree::robot::ChannelPublisherPtr<MessageType>;

  MessageType msg_;

  explicit RealTimePublisher(PublisherSharedPtr publisher)
  : publisher_(publisher), is_running_(false), keep_running_(true), turn_(LOOP_NOT_STARTED)
  {
    thread_ = std::thread(&RealTimePublisher::publishingLoop, this);
  }

  explicit RealTimePublisher(std::string topic)
  : RealTimePublisher(std::make_shared<PublisherBase<MsgType>>(topic))
  {}

  ~RealTimePublisher()
  {
    stop();
    while (is_running()) {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    if(thread_.joinable()) { thread_.join(); }
  }

  void stop()
  {
    keep_running_ = false;
  }

  /**
   * @brief Try to get the data lock from realtime
   * 
   * To publish data from the realtime loop, you need to run trylock to
   * attenot to get unique access to the msg_ variable. Teylock returns
   * true if the lock was aquired, and false otherwise.
   */
  bool trylock()
  {
    if(mutex_.try_lock())
    {
      if(turn_ == REALTIME) {
        return true;
      } else {
        mutex_.unlock();
        return false;
      }
    } else {
      return false;
    }
  }

  /**
   * @brief Unlock the msg_ variable and publish it.
   */
  void unlockAndPublish() 
  {
    turn_ = NON_REALTIME;
    mutex_.unlock();
  }

  /**
   * @brief Get the data lock from non-realtime.
   * 
   * To publish data from the realtime loop, you need to run trylock to
   * attenot to get unique access to the msg_ variable. Teylock returns
   * true if the lock was aquired, and false otherwise.
   */
  void lock()
  {
    // never actually lock on the lock
    while (!mutex_.try_lock()) {
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  }

  /**
   * @brief Unlocks the data without publishing anything.
   */
  void unlock() { mutex_.unlock(); }

protected:
  /**
   * @brief Something before sending the message.
   */
  virtual void post_communication() {}

private:
  // non-copyable
  RealTimePublisher(const RealTimePublisher&) = delete;
  RealTimePublisher& operator=(const RealTimePublisher&) = delete;


  bool is_running() const { return is_running_; }

  void publishingLoop()
  {
    is_running_ = true;
    turn_ = REALTIME;

    while (keep_running_)
    {
      MsgType outgoing;

      // Locks msg_ and copies it
      lock();
      while (turn_ != NON_REALTIME && keep_running_)
      {
        unlock();
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
        lock();
      }
      post_communication();
      outgoing = msg_;
      turn_ = REALTIME;

      unlock();

      if(keep_running_) { 
        publisher_->Write(outgoing, 0); 
      }
    }
    is_running_ = false;
  }

  PublisherSharedPtr publisher_;
  std::atomic_bool is_running_;
  std::atomic_bool keep_running_;

  std::mutex mutex_;

  std::thread thread_;

  enum { REALTIME, NON_REALTIME, LOOP_NOT_STARTED };
  std::atomic<int> turn_;
};

} // namespace robot
} // namespace unitree

#endif // _UT_ROBOT_PUBLISHER_H_