topic.hpp

#pragma once
 
#include <atomic>
#include <concepts>
#include <cstddef>
#include <cstdint>
 
#include "libxr_cb.hpp"
#include "libxr_def.hpp"
#include "libxr_time.hpp"
#include "libxr_type.hpp"
#include "lockfree_list.hpp"
#include "mutex.hpp"
#include "queue.hpp"
#include "rbt.hpp"
#include "semaphore.hpp"
#include "thread.hpp"
 
namespace LibXR
{
template <typename Data>
concept TopicPayload =
    !std::is_reference_v<Data> && !std::is_const_v<Data> && !std::is_volatile_v<Data> &&
    std::is_object_v<Data> && std::is_default_constructible_v<Data> &&
    std::is_copy_assignable_v<Data> && std::is_trivially_destructible_v<Data>;
 
template <typename Data>
constexpr void CheckTopicPayload()
{
  static_assert(TopicPayload<Data>,
                "LibXR::Topic typed payload must be a non-cv/ref object type that is "
                "default-constructible, copy-assignable, and trivially destructible.");
}
 
class Topic
{
  enum class LockState : uint32_t
  {
    UNLOCKED = 0,  
    LOCKED = 1,    
    USE_MUTEX = UINT32_MAX  
  };
 
 public:
  struct Block
  {
    std::atomic<LockState>
        busy;             
    LockFreeList subers;  
    TypeID::ID
        payload_type_id;    
    uint32_t payload_size;  
    uint32_t payload_alignment;  
    uint32_t crc32;              
    Mutex* mutex;  
  };
 
#ifndef __DOXYGEN__
  struct PackedDataHeader;
 
  template <typename Data>
  class PackedData;
  static constexpr uint8_t PACKET_PREFIX =
      0x5A;  
  static constexpr uint8_t PACKET_VERSION =
      0x01;  
  static constexpr size_t PACK_BASE_SIZE =
      17;  
#endif
 
  typedef RBTree<uint32_t>::Node<Block>* TopicHandle;
 
  template <typename Data>
  struct MessageView
  {
    static_assert(TopicPayload<Data>);
    MicrosecondTimestamp timestamp;  
    Data* data;  
  };
 
  struct RawMessageView
  {
    MicrosecondTimestamp timestamp;  
    ConstRawData payload;  
  };
 
  template <typename Data>
  struct Message
  {
    static_assert(TopicPayload<Data>);
    MicrosecondTimestamp timestamp;  
    Data data;                       
  };
 
  static void Lock(TopicHandle topic);
 
  static void Unlock(TopicHandle topic);
 
  static void LockFromCallback(TopicHandle topic);
 
  static void UnlockFromCallback(TopicHandle topic);
 
  class Domain
  {
   public:
    Domain(const char* name);
 
    RBTree<uint32_t>::Node<RBTree<uint32_t>>*
        node_;  
  };
 
  enum class SuberType : uint8_t
  {
    SYNC,      
    ASYNC,     
    QUEUE,     
    CALLBACK,  
  };
 
  struct SuberBlock
  {
    SuberType type;  
  };
 
  struct SyncBlock;
 
  template <typename Data>
  class SyncSubscriber;
 
  enum class ASyncSubscriberState : uint32_t;
 
  struct ASyncBlock;
 
  template <typename Data>
  class ASyncSubscriber;
 
  struct QueueBlock;
 
  class QueuedSubscriber;
 
  class Callback;
 
  struct CallbackBlock;
 
  class Server;
 
  void RegisterCallback(Callback& cb);
 
  static MicrosecondTimestamp NowTimestamp();
 
  Topic();
 
  Topic(const char* name, TypeID::ID payload_type_id, size_t payload_size,
        size_t payload_alignment, Domain* domain = nullptr, bool multi_publisher = false);
 
  template <typename Data>
  static Topic CreateTopic(const char* name, Domain* domain = nullptr,
                           bool multi_publisher = false)
  {
    CheckTopicPayload<Data>();
    return Topic(name, TypeID::GetID<Data>(), sizeof(Data), alignof(Data), domain,
                 multi_publisher);
  }
 
  Topic(TopicHandle topic);
 
  static TopicHandle Find(const char* name, Domain* domain = nullptr);
 
  template <typename Data>
  static TopicHandle FindOrCreate(const char* name, Domain* domain = nullptr,
                                  bool multi_publisher = false)
  {
    CheckTopicPayload<Data>();
    auto topic = Find(name, domain);
    if (topic != nullptr)
    {
      RequireTypeContract(topic, TypeID::GetID<Data>(), sizeof(Data), alignof(Data));
      if (multi_publisher)
      {
        RequireMultiPublisher(topic);
      }
    }
    else
    {
      topic = CreateTopic<Data>(name, domain, multi_publisher).block_;
    }
    return topic;
  }
 
  [[nodiscard]] size_t PayloadSize() const
  {
    ASSERT(block_ != nullptr);
    return block_->data_.payload_size;
  }
 
  [[nodiscard]] size_t PayloadAlignment() const
  {
    ASSERT(block_ != nullptr);
    return block_->data_.payload_alignment;
  }
 
  template <typename Data>
  void Publish(Data& data)
  {
    PublishTyped(data, NowTimestamp(), false, false);
  }
 
  template <typename Data>
  void Publish(Data& data, MicrosecondTimestamp timestamp)
  {
    PublishTyped(data, timestamp, false, false);
  }
 
  template <typename Data>
  void PublishFromCallback(Data& data, bool in_isr)
  {
    PublishTyped(data, NowTimestamp(), true, in_isr);
  }
 
  template <typename Data>
  void PublishFromCallback(Data& data, MicrosecondTimestamp timestamp, bool in_isr)
  {
    PublishTyped(data, timestamp, true, in_isr);
  }
 
  void PublishBytesFromServer(void* payload_addr, size_t payload_size,
                              MicrosecondTimestamp timestamp)
  {
    PublishServerBytes(payload_addr, payload_size, timestamp, false, false);
  }
 
  void PublishBytesFromServerCallback(void* payload_addr, size_t payload_size,
                                      MicrosecondTimestamp timestamp, bool in_isr)
  {
    PublishServerBytes(payload_addr, payload_size, timestamp, true, in_isr);
  }
 
  template <typename Data>
  ErrorCode PackData(const Data& data, PackedData<Data>& packet)
  {
    return PackData(data, packet, NowTimestamp());
  }
 
  template <typename Data>
  ErrorCode PackData(const Data& data, PackedData<Data>& packet,
                     MicrosecondTimestamp timestamp);
 
  ErrorCode PackRaw(ConstRawData data, RawData packet)
  {
    return PackRaw(data, packet, NowTimestamp());
  }
 
  ErrorCode PackRaw(ConstRawData data, RawData packet, MicrosecondTimestamp timestamp)
  {
    if (block_ == nullptr || data.addr_ == nullptr || packet.addr_ == nullptr)
    {
      return ErrorCode::PTR_NULL;
    }
 
    if (data.size_ != block_->data_.payload_size)
    {
      return ErrorCode::SIZE_ERR;
    }
 
    if (packet.size_ < PACK_BASE_SIZE + data.size_)
    {
      return ErrorCode::NO_BUFF;
    }
 
    PackBytes(block_->data_.crc32, packet, timestamp, data);
    return ErrorCode::OK;
  }
 
  static TopicHandle WaitTopic(const char* name, uint32_t timeout = UINT32_MAX,
                               Domain* domain = nullptr);
 
  operator TopicHandle() { return block_; }
 
  uint32_t GetKey() const;
 
 private:
  TopicHandle block_ = nullptr;  
 
  static inline RBTree<uint32_t>* domain_ =
      nullptr;  
  static inline Domain* def_domain_ = nullptr;  
 
  static void EnsureDomainRegistry();
 
  static Domain* EnsureDefaultDomain();
 
  static void CheckServerPublishContract(TopicHandle topic, void* payload_addr,
                                         size_t payload_size)
  {
    ASSERT(topic != nullptr);
    ASSERT(payload_addr != nullptr);
    ASSERT(payload_size == topic->data_.payload_size);
    ASSERT(topic->data_.payload_alignment != 0);
    ASSERT(reinterpret_cast<uintptr_t>(payload_addr) % topic->data_.payload_alignment ==
           0);
  }
 
  template <typename Data>
  static void CheckSubscriberType(Topic topic)
  {
    CheckTopicPayload<Data>();
    ASSERT(topic.block_ != nullptr);
    RequireTypeContract(topic.block_, TypeID::GetID<Data>(), sizeof(Data), alignof(Data));
  }
 
  static void RequireTypeContract(TopicHandle topic, TypeID::ID payload_type_id,
                                  size_t payload_size, size_t payload_alignment);
 
  static void RequireCallbackType(TopicHandle topic, TypeID::ID payload_type_id);
 
  static void RequireMultiPublisher(TopicHandle topic);
 
  template <typename Data>
  static void* AllocateSubscriberBuffer()
  {
    CheckTopicPayload<Data>();
    return new Data;
  }
 
  template <typename Data>
  static void CopyPayload(void* dst, void* payload_addr)
  {
    CheckTopicPayload<Data>();
    DEV_ASSERT(dst != nullptr);
    DEV_ASSERT(payload_addr != nullptr);
    *reinterpret_cast<Data*>(dst) = *reinterpret_cast<Data*>(payload_addr);
  }
 
  template <typename Data>
  void PublishTyped(Data& data, MicrosecondTimestamp timestamp, bool from_callback,
                    bool in_isr)
  {
    CheckTopicPayload<Data>();
 
    if (from_callback)
    {
      LockFromCallback(block_);
    }
    else
    {
      Lock(block_);
    }
 
    CheckPublishContract(block_, TypeID::GetID<Data>(), sizeof(Data), alignof(Data));
    DispatchSubscribers(block_, timestamp, &data, from_callback, in_isr);
 
    if (from_callback)
    {
      UnlockFromCallback(block_);
    }
    else
    {
      Unlock(block_);
    }
  }
 
  static void CheckPublishContract(TopicHandle topic, TypeID::ID payload_type_id,
                                   size_t payload_size, size_t payload_alignment);
 
  static void PackBytes(uint32_t topic_name_crc32, RawData buffer,
                        MicrosecondTimestamp timestamp, ConstRawData data);
 
  static void DispatchSubscriber(SuberBlock& block, MicrosecondTimestamp timestamp,
                                 void* payload_addr, bool from_callback, bool in_isr);
 
  static void DispatchSubscribers(TopicHandle topic, MicrosecondTimestamp timestamp,
                                  void* payload_addr, bool from_callback, bool in_isr);
 
  void PublishServerBytes(void* payload_addr, size_t payload_size,
                          MicrosecondTimestamp timestamp, bool from_callback, bool in_isr)
  {
    CheckServerPublishContract(block_, payload_addr, payload_size);
 
    if (from_callback)
    {
      LockFromCallback(block_);
    }
    else
    {
      Lock(block_);
    }
 
    DispatchSubscribers(block_, timestamp, payload_addr, from_callback, in_isr);
 
    if (from_callback)
    {
      UnlockFromCallback(block_);
    }
    else
    {
      Unlock(block_);
    }
  }
};
}  // namespace LibXR
 
#include "packet/packet.hpp"
#include "server/server.hpp"
#include "subscriber/async.hpp"
#include "subscriber/callback.hpp"
#include "subscriber/queue.hpp"
#include "subscriber/sync.hpp"