// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__SUBSCRIPTION_HPP_
#define RCLCPP__SUBSCRIPTION_HPP_

#include <rmw/error_handling.h>
#include <rmw/rmw.h>

#include <chrono>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <utility>

#include "rcl/error_handling.h"
#include "rcl/subscription.h"

#include "rclcpp/any_subscription_callback.hpp"
#include "rclcpp/detail/resolve_use_intra_process.hpp"
#include "rclcpp/detail/resolve_intra_process_buffer_type.hpp"
#include "rclcpp/exceptions.hpp"
#include "rclcpp/expand_topic_or_service_name.hpp"
#include "rclcpp/experimental/intra_process_manager.hpp"
#include "rclcpp/experimental/subscription_intra_process.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/message_info.hpp"
#include "rclcpp/message_memory_strategy.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/subscription_options.hpp"
#include "rclcpp/subscription_traits.hpp"
#include "rclcpp/type_support_decl.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rclcpp/waitable.hpp"
#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"
#include "tracetools/tracetools.h"

namespace rclcpp
{

namespace node_interfaces
{
class NodeTopicsInterface;
}  // namespace node_interfaces

/// Subscription implementation, templated on the type of message this subscription receives.
template<
  typename MessageT,
  typename AllocatorT = std::allocator<void>,
  /// MessageT::custom_type if MessageT is a TypeAdapter,
  /// otherwise just MessageT.
  typename SubscribedT = typename rclcpp::TypeAdapter<MessageT>::custom_type,
  /// MessageT::ros_message_type if MessageT is a TypeAdapter,
  /// otherwise just MessageT.
  typename ROSMessageT = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
  typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
    ROSMessageT,
    AllocatorT
  >>
class Subscription : public SubscriptionBase
{
  friend class rclcpp::node_interfaces::NodeTopicsInterface;

public:
  // Redeclare these here to use outside of the class.
  using SubscribedType = SubscribedT;
  using ROSMessageType = ROSMessageT;
  using MessageMemoryStrategyType = MessageMemoryStrategyT;

  using SubscribedTypeAllocatorTraits = allocator::AllocRebind<SubscribedType, AllocatorT>;
  using SubscribedTypeAllocator = typename SubscribedTypeAllocatorTraits::allocator_type;
  using SubscribedTypeDeleter = allocator::Deleter<SubscribedTypeAllocator, SubscribedType>;

  using ROSMessageTypeAllocatorTraits = allocator::AllocRebind<ROSMessageType, AllocatorT>;
  using ROSMessageTypeAllocator = typename ROSMessageTypeAllocatorTraits::allocator_type;
  using ROSMessageTypeDeleter = allocator::Deleter<ROSMessageTypeAllocator, ROSMessageType>;

private:
  using SubscriptionTopicStatisticsSharedPtr =
    std::shared_ptr<rclcpp::topic_statistics::SubscriptionTopicStatistics>;

public:
  RCLCPP_SMART_PTR_DEFINITIONS(Subscription)

  /// Default constructor.
  /**
   * The constructor for a subscription is almost never called directly.
   * Instead, subscriptions should be instantiated through the function
   * rclcpp::create_subscription().
   *
   * \param[in] node_base NodeBaseInterface pointer that is used in part of the setup.
   * \param[in] type_support_handle rosidl type support struct, for the Message type of the topic.
   * \param[in] topic_name Name of the topic to subscribe to.
   * \param[in] qos QoS profile for Subcription.
   * \param[in] callback User defined callback to call when a message is received.
   * \param[in] options Options for the subscription.
   * \param[in] message_memory_strategy The memory strategy to be used for managing message memory.
   * \param[in] subscription_topic_statistics Optional pointer to a topic statistics subcription.
   * \throws std::invalid_argument if the QoS is uncompatible with intra-process (if one
   *   of the following conditions are true: qos_profile.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL,
   *   qos_profile.depth == 0 or qos_profile.durability != RMW_QOS_POLICY_DURABILITY_VOLATILE).
   */
  // *INDENT-OFF*
  Subscription(
    rclcpp::node_interfaces::NodeBaseInterface * node_base,
    const rosidl_message_type_support_t & type_support_handle,
    const std::string & topic_name,
    const rclcpp::QoS & qos,
    AnySubscriptionCallback<MessageT, AllocatorT> callback,
    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
    typename MessageMemoryStrategyT::SharedPtr message_memory_strategy,
    SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr)
  : SubscriptionBase(
      node_base,
      type_support_handle,
      topic_name,
      options.to_rcl_subscription_options(qos),
      // NOTE(methylDragon): Passing these args separately is necessary for event binding
      options.event_callbacks,
      options.use_default_callbacks,
      callback.is_serialized_message_callback() ? DeliveredMessageKind::SERIALIZED_MESSAGE : DeliveredMessageKind::ROS_MESSAGE),  // NOLINT
    any_callback_(callback),
    options_(options),
    message_memory_strategy_(message_memory_strategy)
  // *INDENT-ON*
  {
    // Setup intra process publishing if requested.
    if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) {
      using rclcpp::detail::resolve_intra_process_buffer_type;

      // Check if the QoS is compatible with intra-process.
      auto qos_profile = get_actual_qos();
      if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) {
        throw std::invalid_argument(
                "intraprocess communication on topic '" + topic_name +
                "' allowed only with keep last history qos policy");
      }
      if (qos_profile.depth() == 0) {
        throw std::invalid_argument(
                "intraprocess communication on topic '" + topic_name +
                "' is not allowed with 0 depth qos policy");
      }

      using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess<
        MessageT,
        SubscribedType,
        SubscribedTypeAllocator,
        SubscribedTypeDeleter,
        ROSMessageT,
        AllocatorT>;

      // Build a type-erased stats handler to avoid a circular include chain
      // via publisher.hpp and callback_group.hpp
      typename SubscriptionIntraProcessT::StatsHandlerFn stats_handler = nullptr;
      if (subscription_topic_statistics) {
        stats_handler =
          [subscription_topic_statistics](
          const rmw_message_info_t & info, const rclcpp::Time & time)
          {
            subscription_topic_statistics->handle_message(info, time);
          };
      }

      // First create a SubscriptionIntraProcess which will be given to the intra-process manager.
      auto context = node_base->get_context();
      subscription_intra_process_ = std::make_shared<SubscriptionIntraProcessT>(
        callback,
        options_.get_allocator(),
        context,
        this->get_topic_name(),  // important to get like this, as it has the fully-qualified name
        qos_profile,
        resolve_intra_process_buffer_type(options_.intra_process_buffer_type, callback),
        std::move(stats_handler));
      TRACETOOLS_TRACEPOINT(
        rclcpp_subscription_init,
        static_cast<const void *>(get_subscription_handle().get()),
        static_cast<const void *>(subscription_intra_process_.get()));

      // Add it to the intra process manager.
      using rclcpp::experimental::IntraProcessManager;
      auto ipm = context->get_sub_context<IntraProcessManager>();
      uint64_t intra_process_subscription_id = ipm->template add_subscription<
        ROSMessageType, ROSMessageTypeAllocator>(subscription_intra_process_);
      this->setup_intra_process(intra_process_subscription_id, ipm);
    }

    if (subscription_topic_statistics != nullptr) {
      this->subscription_topic_statistics_ = std::move(subscription_topic_statistics);
    }

    TRACETOOLS_TRACEPOINT(
      rclcpp_subscription_init,
      static_cast<const void *>(get_subscription_handle().get()),
      static_cast<const void *>(this));
    TRACETOOLS_TRACEPOINT(
      rclcpp_subscription_callback_added,
      static_cast<const void *>(this),
      static_cast<const void *>(&any_callback_));
    // The callback object gets copied, so if registration is done too early/before this point
    // (e.g. in `AnySubscriptionCallback::set()`), its address won't match any address used later
    // in subsequent tracepoints.
#ifndef TRACETOOLS_DISABLED
    any_callback_.register_callback_for_tracing();
#endif
  }

  /// Called after construction to continue setup that requires shared_from_this().
  void
  post_init_setup(
    [[maybe_unused]] rclcpp::node_interfaces::NodeBaseInterface * node_base,
    [[maybe_unused]] const rclcpp::QoS & qos,
    [[maybe_unused]] const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options)
  {
    // This function is intentionally left empty.
  }

  /// Take the next message from the inter-process subscription.
  /**
   * Data may be taken (written) into the message_out and message_info_out even
   * if false is returned.
   * Specifically in the case of dropping redundant intra-process data, where
   * data is received via both intra-process and inter-process (due to the
   * underlying middleware being unabled to avoid this duplicate delivery) and
   * so inter-process data from those intra-process publishers is ignored, but
   * it has to be taken to know if it came from an intra-process publisher or
   * not, and therefore could be dropped.
   *
   * \sa SubscriptionBase::take_type_erased()
   *
   * \param[out] message_out The message into which take will copy the data.
   * \param[out] message_info_out The message info for the taken message.
   * \returns true if data was taken and is valid, otherwise false
   * \throws any rcl errors from rcl_take, \sa rclcpp::exceptions::throw_from_rcl_error()
   */
  bool
  take(ROSMessageType & message_out, rclcpp::MessageInfo & message_info_out)
  {
    return this->take_type_erased(static_cast<void *>(&message_out), message_info_out);
  }

  /// Take the next message from the inter-process subscription.
  /**
   * This version takes a SubscribedType which is different from the
   * ROSMessageType when a rclcpp::TypeAdapter is in used.
   *
   * \sa take(ROSMessageType &, rclcpp::MessageInfo &)
   */
  template<typename TakeT>
  std::enable_if_t<
    !rosidl_generator_traits::is_message<TakeT>::value &&
    std::is_same_v<TakeT, SubscribedType>,
    bool
  >
  take(TakeT & message_out, rclcpp::MessageInfo & message_info_out)
  {
    ROSMessageType local_message;
    bool taken = this->take_type_erased(static_cast<void *>(&local_message), message_info_out);
    if (taken) {
      rclcpp::TypeAdapter<MessageT>::convert_to_custom(local_message, message_out);
    }
    return taken;
  }

  std::shared_ptr<void>
  create_message() override
  {
    /* The default message memory strategy provides a dynamically allocated message on each call to
     * create_message, though alternative memory strategies that re-use a preallocated message may be
     * used (see rclcpp/strategies/message_pool_memory_strategy.hpp).
     */
    return message_memory_strategy_->borrow_message();
  }

  std::shared_ptr<rclcpp::SerializedMessage>
  create_serialized_message() override
  {
    return message_memory_strategy_->borrow_serialized_message();
  }

  /// Disable callbacks from being called
  /**
    * This method will block, until any subscription's callbacks provided during construction
    * currently being executed are finished.
    * \note This method also temporary removes the on new message callback and all
    * on new event callbacks from the rmw layer to prevent them from being called. However, this
    * method will not block and wait until the currently executing on_new_[message]event callbacks
    * are finished.
    */
  void
  disable_callbacks() override
  {
    SubscriptionBase::disable_callbacks();
    any_callback_.disable();
    if (subscription_intra_process_) {
      subscription_intra_process_->disable_callbacks();
    }
    for (const auto & [_, event_ptr] : event_handlers_) {
      if (event_ptr) {
        event_ptr->disable();
      }
    }
  }

  /// Enable the callbacks to be called
  /**
    * This method is thread safe, and provides a safe way to atomically enable the callbacks
    * in a multithreaded environment.
    */
  void
  enable_callbacks() override
  {
    SubscriptionBase::enable_callbacks();
    any_callback_.enable();
    if (subscription_intra_process_) {
      subscription_intra_process_->enable_callbacks();
    }
    for (const auto & [_, event_ptr] : event_handlers_) {
      if (event_ptr) {
        event_ptr->enable();
      }
    }
  }

  void
  handle_message(
    std::shared_ptr<void> & message,
    const rclcpp::MessageInfo & message_info) override
  {
    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
      // In this case, the message will be delivered via intra process and
      // we should ignore this copy of the message.
      return;
    }
    auto typed_message = std::static_pointer_cast<ROSMessageType>(message);

    std::chrono::time_point<std::chrono::system_clock> now;
    if (subscription_topic_statistics_) {
      // get current time before executing callback to
      // exclude callback duration from topic statistics result.
      now = std::chrono::system_clock::now();
    }

    any_callback_.dispatch(typed_message, message_info);

    if (subscription_topic_statistics_) {
      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
    }
  }

  void
  handle_serialized_message(
    const std::shared_ptr<rclcpp::SerializedMessage> & serialized_message,
    const rclcpp::MessageInfo & message_info) override
  {
    std::chrono::time_point<std::chrono::system_clock> now;
    if (subscription_topic_statistics_) {
      // get current time before executing callback to
      // exclude callback duration from topic statistics result.
      now = std::chrono::system_clock::now();
    }

    any_callback_.dispatch(serialized_message, message_info);

    if (subscription_topic_statistics_) {
      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
    }
  }

  void
  handle_loaned_message(
    void * loaned_message,
    const rclcpp::MessageInfo & message_info) override
  {
    if (matches_any_intra_process_publishers(&message_info.get_rmw_message_info().publisher_gid)) {
      // In this case, the message will be delivered via intra process and
      // we should ignore this copy of the message.
      return;
    }

    auto typed_message = static_cast<ROSMessageType *>(loaned_message);
    // message is loaned, so we have to make sure that the deleter does not deallocate the message
    auto sptr = std::shared_ptr<ROSMessageType>(
      typed_message, [](ROSMessageType * msg) {(void) msg;});

    std::chrono::time_point<std::chrono::system_clock> now;
    if (subscription_topic_statistics_) {
      // get current time before executing callback to
      // exclude callback duration from topic statistics result.
      now = std::chrono::system_clock::now();
    }

    any_callback_.dispatch(sptr, message_info);

    if (subscription_topic_statistics_) {
      const auto nanos = std::chrono::time_point_cast<std::chrono::nanoseconds>(now);
      const auto time = rclcpp::Time(nanos.time_since_epoch().count());
      subscription_topic_statistics_->handle_message(message_info.get_rmw_message_info(), time);
    }
  }

  /// Return the borrowed message.
  /**
   * \param[inout] message message to be returned
   */
  void
  return_message(std::shared_ptr<void> & message) override
  {
    auto typed_message = std::static_pointer_cast<ROSMessageType>(message);
    message_memory_strategy_->return_message(typed_message);
  }

  /// Return the borrowed serialized message.
  /**
   * \param[inout] message serialized message to be returned
   */
  void
  return_serialized_message(std::shared_ptr<rclcpp::SerializedMessage> & message) override
  {
    message_memory_strategy_->return_serialized_message(message);
  }

  bool
  use_take_shared_method() const
  {
    return any_callback_.use_take_shared_method();
  }

  // DYNAMIC TYPE ==================================================================================
  // TODO(methylDragon): Reorder later
  // TODO(methylDragon): Implement later...
  rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
  get_shared_dynamic_message_type() override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "get_shared_dynamic_message_type is not implemented for Subscription");
  }

  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
  get_shared_dynamic_message() override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "get_shared_dynamic_message is not implemented for Subscription");
  }

  rclcpp::dynamic_typesupport::DynamicSerializationSupport::SharedPtr
  get_shared_dynamic_serialization_support() override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "get_shared_dynamic_serialization_support is not implemented for Subscription");
  }

  rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr
  create_dynamic_message() override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "create_dynamic_message is not implemented for Subscription");
  }

  void
  return_dynamic_message(
    [[maybe_unused]] rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message) override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "return_dynamic_message is not implemented for Subscription");
  }

  void
  handle_dynamic_message(
    [[maybe_unused]] const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr & message,
    [[maybe_unused]] const rclcpp::MessageInfo & message_info) override
  {
    throw rclcpp::exceptions::UnimplementedError(
            "handle_dynamic_message is not implemented for Subscription");
  }

private:
  RCLCPP_DISABLE_COPY(Subscription)

  AnySubscriptionCallback<MessageT, AllocatorT> any_callback_;
  /// Copy of original options passed during construction.
  /**
   * It is important to save a copy of this so that the rmw payload which it
   * may contain is kept alive for the duration of the subscription.
   */
  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> options_;
  typename message_memory_strategy::MessageMemoryStrategy<ROSMessageType, AllocatorT>::SharedPtr
    message_memory_strategy_;

  /// Component which computes and publishes topic statistics for this subscriber
  SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr};
};

}  // namespace rclcpp

#endif  // RCLCPP__SUBSCRIPTION_HPP_
