callback.hpp

#pragma once
 
#include <new>
 
#include "../topic.hpp"
#include "libxr_def.hpp"
 
namespace LibXR
{
class Topic::Callback
{
  template <typename Function>
  struct FunctionTraits;
 
  template <typename Return, typename... Args>
  struct FunctionTraits<Return (*)(Args...)>
  {
    using ReturnType = Return;  
    static constexpr size_t ARITY = sizeof...(Args);  
 
    template <size_t Index>
    using Arg = std::tuple_element_t<Index, std::tuple<Args...>>;
  };
 
  template <typename T>
  struct MessageViewTraits
  {
    static constexpr bool VALUE =
        false;  
  };
 
  template <typename Data>
  struct MessageViewTraits<MessageView<Data>>
  {
    static constexpr bool VALUE =
        true;  
    using DataType =
        Data;  
  };
 
  template <typename T>
  using RemoveCVRef = std::remove_cv_t<std::remove_reference_t<T>>;
 
  template <typename T>
  static constexpr bool IS_MESSAGE_VIEW = MessageViewTraits<
      RemoveCVRef<T>>::VALUE;  
 
  template <typename T>
  static constexpr bool IS_RAW_DATA_VIEW = std::same_as<RemoveCVRef<T>, ConstRawData> ||
                                           std::same_as<RemoveCVRef<T>, RawMessageView>;
 
  template <typename T>
  static constexpr bool IS_TYPED_DATA =
      !IS_MESSAGE_VIEW<T> && !IS_RAW_DATA_VIEW<T> &&
      TopicPayload<RemoveCVRef<T>>;  
 
  template <typename T>
  static constexpr bool IS_CALLBACK_PAYLOAD =
      IS_MESSAGE_VIEW<T> || IS_RAW_DATA_VIEW<T> ||
      IS_TYPED_DATA<T>;  
 
  template <typename T>
  static constexpr bool IS_MUTABLE_MESSAGE_VIEW_REF =
      IS_MESSAGE_VIEW<T> && std::is_lvalue_reference_v<T> &&
      !std::is_const_v<std::remove_reference_t<T>>;
 
  struct BlockHeader
  {
    using RunFun = void (*)(const BlockHeader*, bool, MicrosecondTimestamp, void*,
                            size_t);  
 
    RunFun run = nullptr;  
    TypeID::ID payload_type_id = nullptr;  
    bool accepts_raw_payload = false;      
  };
 
  template <typename PayloadArg>
  static TypeID::ID PayloadTypeID()
  {
    static_assert(!IS_MUTABLE_MESSAGE_VIEW_REF<PayloadArg>,
                  "LibXR::Topic::Callback does not accept MessageView<T>&; "
                  "use MessageView<T> or const MessageView<T>& instead.");
 
    if constexpr (IS_RAW_DATA_VIEW<PayloadArg>)
    {
      return nullptr;
    }
    else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
    {
      using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
      return TypeID::GetID<typename View::DataType>();
    }
    else
    {
      static_assert(IS_TYPED_DATA<PayloadArg>,
                    "LibXR::Topic::Callback payload must be Topic::MessageView<T>, "
                    "Topic::RawMessageView, ConstRawData, T, T&, or const T&.");
      return TypeID::GetID<RemoveCVRef<PayloadArg>>();
    }
  }
 
  template <typename PayloadArg>
  static constexpr bool AcceptsRawPayload()
  {
    return IS_RAW_DATA_VIEW<PayloadArg>;
  }
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  static void InvokePayload(Function fun, BoundArg& arg, bool in_isr,
                            MicrosecondTimestamp timestamp, void* payload_addr,
                            size_t payload_size)
  {
    if constexpr (std::same_as<RemoveCVRef<PayloadArg>, ConstRawData>)
    {
      ConstRawData raw_payload{payload_addr, payload_size};
      fun(in_isr, arg, raw_payload);
    }
    else if constexpr (std::same_as<RemoveCVRef<PayloadArg>, RawMessageView>)
    {
      RawMessageView raw_message{timestamp, ConstRawData{payload_addr, payload_size}};
      fun(in_isr, arg, raw_message);
    }
    else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
    {
      using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
      using Data = typename View::DataType;
      fun(in_isr, arg,
          MessageView<Data>{timestamp, reinterpret_cast<Data*>(payload_addr)});
    }
    else
    {
      using Data = RemoveCVRef<PayloadArg>;
      fun(in_isr, arg, *reinterpret_cast<Data*>(payload_addr));
    }
  }
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  struct PayloadOnlyBlock : public BlockHeader
  {
    PayloadOnlyBlock(Function fun, BoundArg&& arg)
        : BlockHeader{&Run, PayloadTypeID<PayloadArg>(), AcceptsRawPayload<PayloadArg>()},
          fun_(fun),
          arg_(std::move(arg))
    {
    }
 
    static void Run(const BlockHeader* header, bool in_isr,
                    MicrosecondTimestamp timestamp, void* payload_addr,
                    size_t payload_size)
    {
      auto* block = static_cast<const PayloadOnlyBlock*>(header);
      InvokePayload<Function, BoundArg, PayloadArg>(
          block->fun_, const_cast<BoundArg&>(block->arg_), in_isr, timestamp,
          payload_addr, payload_size);
    }
 
    Function fun_;  
    BoundArg arg_;  
  };
 
  template <typename Function, typename BoundArg, typename PayloadArg>
  struct TimestampPayloadBlock : public BlockHeader
  {
    TimestampPayloadBlock(Function fun, BoundArg&& arg)
        : BlockHeader{&Run, PayloadTypeID<PayloadArg>(), AcceptsRawPayload<PayloadArg>()},
          fun_(fun),
          arg_(std::move(arg))
    {
    }
 
    static void Run(const BlockHeader* header, bool in_isr,
                    MicrosecondTimestamp timestamp, void* payload_addr,
                    size_t payload_size)
    {
      auto* block = static_cast<const TimestampPayloadBlock*>(header);
      if constexpr (std::same_as<RemoveCVRef<PayloadArg>, ConstRawData>)
      {
        ConstRawData raw_payload{payload_addr, payload_size};
        block->fun_(in_isr, block->arg_, timestamp, raw_payload);
      }
      else if constexpr (std::same_as<RemoveCVRef<PayloadArg>, RawMessageView>)
      {
        RawMessageView raw_message{timestamp, ConstRawData{payload_addr, payload_size}};
        block->fun_(in_isr, block->arg_, timestamp, raw_message);
      }
      else if constexpr (IS_MESSAGE_VIEW<PayloadArg>)
      {
        using View = MessageViewTraits<RemoveCVRef<PayloadArg>>;
        using Data = typename View::DataType;
        block->fun_(in_isr, block->arg_, timestamp,
                    MessageView<Data>{timestamp, reinterpret_cast<Data*>(payload_addr)});
      }
      else
      {
        using Data = RemoveCVRef<PayloadArg>;
        block->fun_(in_isr, block->arg_, timestamp,
                    *reinterpret_cast<Data*>(payload_addr));
      }
    }
 
    Function fun_;  
    BoundArg arg_;  
  };
 
  template <typename Function, typename BoundArg, size_t Arity>
  struct Factory
  {
    static_assert(Arity == 3 || Arity == 4,
                  "LibXR::Topic::Callback function must be void(bool, Arg, Payload) "
                  "or void(bool, Arg, MicrosecondTimestamp, Payload).");
  };
 
  template <typename Function, typename BoundArg>
  struct Factory<Function, BoundArg, 3>
  {
    using Traits = FunctionTraits<Function>;  
    using PayloadArg =
        typename Traits::template Arg<2>;  
 
    static BlockHeader* Create(Function fun, BoundArg&& arg)
    {
      static_assert(std::same_as<typename Traits::ReturnType, void>);
      static_assert(std::same_as<typename Traits::template Arg<0>, bool>);
      static_assert(std::same_as<typename Traits::template Arg<1>, BoundArg>);
      static_assert(IS_CALLBACK_PAYLOAD<PayloadArg>);
      return new PayloadOnlyBlock<Function, BoundArg, PayloadArg>(fun, std::move(arg));
    }
  };
 
  template <typename Function, typename BoundArg>
  struct Factory<Function, BoundArg, 4>
  {
    using Traits = FunctionTraits<Function>;  
    using TimestampArg =
        typename Traits::template Arg<2>;  
    using PayloadArg =
        typename Traits::template Arg<3>;  
 
    static BlockHeader* Create(Function fun, BoundArg&& arg)
    {
      static_assert(std::same_as<typename Traits::ReturnType, void>);
      static_assert(std::same_as<typename Traits::template Arg<0>, bool>);
      static_assert(std::same_as<typename Traits::template Arg<1>, BoundArg>);
      static_assert(std::same_as<RemoveCVRef<TimestampArg>, MicrosecondTimestamp>);
      static_assert(IS_CALLBACK_PAYLOAD<PayloadArg>);
      return new TimestampPayloadBlock<Function, BoundArg, PayloadArg>(fun,
                                                                       std::move(arg));
    }
  };
 
  static void EmptyRun(const BlockHeader* header, bool in_isr,
                       MicrosecondTimestamp timestamp, void* payload_addr,
                       size_t payload_size)
  {
    UNUSED(header);
    UNUSED(in_isr);
    UNUSED(timestamp);
    UNUSED(payload_addr);
    UNUSED(payload_size);
  }
 
  inline static BlockHeader empty_block_{
      &EmptyRun, nullptr, false};  
 
 public:
  Callback() = default;
 
  Callback(const Callback& other) = default;
 
  Callback& operator=(const Callback& other) = default;
 
  template <typename BoundArg, typename Callable>
  [[nodiscard]] static Callback Create(Callable fun, BoundArg arg)
  {
    using Function = decltype(+std::declval<Callable>());
    using Traits = FunctionTraits<Function>;
    static_assert(Traits::ARITY == 3 || Traits::ARITY == 4,
                  "LibXR::Topic::Callback::Create expects a capture-free callable "
                  "with bool and bound-argument parameters.");
    return Callback(
        Factory<Function, BoundArg, Traits::ARITY>::Create(+fun, std::move(arg)));
  }
 
  void Run(bool in_isr, MicrosecondTimestamp timestamp, void* payload_addr,
           size_t payload_size) const
  {
    block_->run(block_, in_isr, timestamp, payload_addr, payload_size);
  }
 
  TypeID::ID PayloadTypeID() const { return block_->payload_type_id; }
 
  bool IsRawPayloadView() const { return block_->accepts_raw_payload; }
 
 private:
  explicit Callback(BlockHeader* block) : block_(block ? block : &empty_block_) {}
 
  BlockHeader* block_ = &empty_block_;  
};
 
struct Topic::CallbackBlock : public Topic::SuberBlock
{
  CallbackBlock(Callback& callback, size_t topic_payload_size)
      : cb(callback), payload_size(topic_payload_size)
  {
    type = SuberType::CALLBACK;
  }
 
  void Run(bool in_isr, MicrosecondTimestamp timestamp, void* payload_addr) const
  {
    cb.Run(in_isr, timestamp, payload_addr, payload_size);
  }
 
  Callback cb;  
 
  size_t payload_size = 0;
};
 
inline void Topic::RegisterCallback(Callback& cb)
{
  if (!cb.IsRawPayloadView())
  {
    ASSERT(block_->data_.payload_type_id == cb.PayloadTypeID());
  }
 
  auto node = new (std::align_val_t(LibXR::CONCURRENCY_ALIGNMENT))
      LockFreeList::Node<CallbackBlock>(cb, block_->data_.payload_size);
  block_->data_.subers.Add(*node);
}
}  // namespace LibXR